#ifndef OPEN_KARTO_KARTO_H
#define OPEN_KARTO_KARTO_H

#include <string>
#include <fstream>
#include <limits>
#include <algorithm>
#include <map>
#include <vector>
#include <iostream>
#include <iomanip>
#include <sstream>
#include <stdexcept>
#include <math.h>
#include <float.h>
#include <stdio.h>
#include <assert.h>
#include <string.h>
#include <boost/thread.hpp>
#include <open_karto/Math.h>
#include <open_karto/Macros.h>

#ifdef USE_POCO
#include <Poco/Mutex.h>
#endif

#define KARTO_Object(name) \
virtual const char* GetClassName() const { return #name; } \
virtual kt_objecttype GetObjectType() const { return ObjectType_##name; }
typedef kt_int32u kt_objecttype;

const kt_objecttype ObjectType_None = 0x00000000;
const kt_objecttype ObjectType_Sensor = 0x00001000;
const kt_objecttype ObjectType_SensorData = 0x00002000;
const kt_objecttype ObjectType_CustomData = 0x00004000;
const kt_objecttype ObjectType_Misc = 0x10000000;
const kt_objecttype ObjectType_Drive = ObjectType_Sensor | 0x01;
const kt_objecttype ObjectType_LaserRangeFinder = ObjectType_Sensor | 0x02;
const kt_objecttype ObjectType_Camera = ObjectType_Sensor | 0x04;
const kt_objecttype ObjectType_DrivePose = ObjectType_SensorData | 0x01;
const kt_objecttype ObjectType_LaserRangeScan = ObjectType_SensorData | 0x02;
const kt_objecttype ObjectType_LocalizedRangeScan = ObjectType_SensorData | 0x04;
const kt_objecttype ObjectType_CameraImage = ObjectType_SensorData | 0x08;
const kt_objecttype ObjectType_LocalizedRangeScanWithPoints = ObjectType_SensorData | 0x16;
const kt_objecttype ObjectType_Header = ObjectType_Misc | 0x01;
const kt_objecttype ObjectType_Parameters = ObjectType_Misc | 0x02;
const kt_objecttype ObjectType_DatasetInfo = ObjectType_Misc | 0x04;
const kt_objecttype ObjectType_Module = ObjectType_Misc | 0x08;

namespace karto
{
/** \defgroup OpenKarto OpenKarto Module
* Exception class. All exceptions thrown from Karto will inherit from this class or be of this class
*/
class KARTO_EXPORT Exception
{
public:
    /**
    * Constructor with exception message
    * @param rMessage exception message (default: "Karto Exception")
    * @param errorCode error code (default: 0)
    */
    Exception(const std::string &rMessage = "Karto Exception", kt_int32s errorCode = 0)
            : m_Message(rMessage), m_ErrorCode(errorCode) {}

    /**
    * Copy constructor
    */
    Exception(const Exception &rException)
            : m_Message(rException.m_Message), m_ErrorCode(rException.m_ErrorCode) {}

    virtual ~Exception() {}

    Exception &operator=(const Exception &rException)
    {
        m_Message = rException.m_Message;
        m_ErrorCode = rException.m_ErrorCode;
        return *this;
    }

    /**
    * Gets the exception message
    * @return error message as string
    */
    const std::string &GetErrorMessage() const { return m_Message; }

    /**
    * Gets error code
    * @return error code
    */
    kt_int32s GetErrorCode() { return m_ErrorCode; }

    /**
    * Write exception to output stream
    * @param rStream output stream
    * @param rException exception to write
    */
    friend KARTO_EXPORT std::ostream &operator<<(std::ostream &rStream, Exception &rException);

private:
    std::string m_Message;
    kt_int32s m_ErrorCode;
};  // class Exception

/**
* Subclass this class to make a non-copyable class (copy
* constructor and assignment operator are private)
*/
class KARTO_EXPORT NonCopyable
{
private:
    NonCopyable(const NonCopyable &);

    const NonCopyable &operator=(const NonCopyable &);

protected:
    NonCopyable() {}

    virtual ~NonCopyable() {}
};  // class NonCopyable

/**
* Singleton class ensures only one instance of T is created
*/
template<class T>
class Singleton
{
public:
    Singleton() : m_pPointer(NULL) {}

    virtual ~Singleton() { delete m_pPointer; }

    /**
    * Gets the singleton
    * @return singleton
    */
    T *Get()
    {
#ifdef USE_POCO
        Poco::FastMutex::ScopedLock lock(m_Mutex);
#endif
        if (m_pPointer == NULL) m_pPointer = new T;
        return m_pPointer;
    }
    
private:
    T *m_pPointer;
#ifdef USE_POCO
    Poco::FastMutex m_Mutex;
#endif

    Singleton(const Singleton &);

    const Singleton &operator=(const Singleton &);
};

////////////////////////////////////////////////////////////////////////////////////////
class KARTO_EXPORT Functor
{
public:
    virtual void operator()(kt_int32u) {};
};  // Functor

////////////////////////////////////////////////////////////////////////////////////////
class AbstractParameter;
/**
* Type declaration of AbstractParameter vector
*/
typedef std::vector<AbstractParameter *> ParameterVector;

class KARTO_EXPORT ParameterManager : public NonCopyable
{
public:
    ParameterManager() {}

    virtual ~ParameterManager() { Clear(); }

    /**
    * Adds the parameter to this manager
    * @param pParameter
    */
    void Add(AbstractParameter *pParameter);

    /**
    * Gets the parameter of the given name
    * @param rName
    * @return parameter of given name
    */
    AbstractParameter *Get(const std::string &rName)
    {
        if (m_ParameterLookup.find(rName) != m_ParameterLookup.end())
        {
            return m_ParameterLookup[rName];
        }
        std::cout << "Unknown parameter: " << rName << std::endl;
        return NULL;
    }

    void Clear();

    /**
    * Gets all parameters
    * @return vector of all parameters
    */
    inline const ParameterVector &GetParameterVector() const
    {
        return m_Parameters;
    }

    /**
    * Gets the parameter with the given name
    * @param rName
    * @return parameter of given name
    */
    AbstractParameter *operator()(const std::string &rName)
    {
        return Get(rName);
    }

private:
    ParameterManager(const ParameterManager &);

    const ParameterManager &operator=(const ParameterManager &);

    ParameterVector m_Parameters;
    std::map<std::string, AbstractParameter *> m_ParameterLookup;
};  // ParameterManager

////////////////////////////////////////////////////////////////////////////////////////
// valid names
// 'Test' -- no scope
// '/Test' -- no scope will be parsed to 'Test'
// '/scope/Test' - 'scope' scope and 'Test' name
// '/scope/name/Test' - 'scope/name' scope and 'Test' name
class Name
{
public:
    Name() {}

    Name(const std::string &rName) { Parse(rName); }

    Name(const Name &rOther) : m_Name(rOther.m_Name), m_Scope(rOther.m_Scope) {}

    virtual ~Name() {}

    /**
    * Gets the name of this name
    * @return name
    */
    inline const std::string &GetName() const { return m_Name; }

    /**
    * Sets the name
    * @param rName name
    */
    inline void SetName(const std::string &rName)
    {
        std::string::size_type pos = rName.find_last_of('/');
        if (pos != 0 && pos != std::string::npos)
        {
            throw Exception("Name can't contain a scope!");
        }
        m_Name = rName;
    }

    /**
    * Gets the scope of this name
    * @return scope
    */
    inline const std::string &GetScope() const { return m_Scope; }

    /**
    * Sets the scope of this name
    * @param rScope scope
    */
    inline void SetScope(const std::string &rScope) { m_Scope = rScope; }

    /**
    * Returns a string representation of this name
    * @return string representation of this name
    */
    inline std::string ToString() const
    {
        if (m_Scope == "") return m_Name;
        else
        {
            std::string name;
            name.append("/");
            name.append(m_Scope);
            name.append("/");
            name.append(m_Name);
            return name;
        }
    }

    Name &operator=(const Name &rOther)
    {
        if (&rOther != this)
        {
            m_Name = rOther.m_Name;
            m_Scope = rOther.m_Scope;
        }
        return *this;
    }

    kt_bool operator==(const Name &rOther) const
    {
        return (m_Name == rOther.m_Name) && (m_Scope == rOther.m_Scope);
    }

    kt_bool operator!=(const Name &rOther) const
    {
        return !(*this == rOther);
    }

    kt_bool operator<(const Name &rOther) const
    {
        return ToString() < rOther.ToString();
    }

    /**
    * Write Name onto output stream
    * @param rStream output stream
    * @param rName to write
    */
    friend inline std::ostream &operator<<(std::ostream &rStream, const Name &rName)
    {
        rStream << rName.ToString();
        return rStream;
    }

private:
    /**
    * Parse the given string into a Name object
    * @param rName name
    */
    void Parse(const std::string &rName)
    {
        std::string::size_type pos = rName.find_last_of('/');

        if (pos == std::string::npos) m_Name = rName;
        else
        {
            m_Scope = rName.substr(0, pos);
            m_Name = rName.substr(pos + 1, rName.size());

            // remove '/' from m_Scope if first!!
            if (m_Scope.size() > 0 && m_Scope[0] == '/')
            {
                m_Scope = m_Scope.substr(1, m_Scope.size());
            }
        }
    }

    /**
    * Validates the given string as a Name
    * @param rName name
    */
    void Validate(const std::string &rName)
    {
        if (rName.empty()) return;
        char c = rName[0];
        if (IsValidFirst(c))
        {
            for (size_t i = 1; i < rName.size(); ++i)
            {
                c = rName[i];
                if (!IsValid(c))
                {
                    throw Exception("Invalid character in name. "
                                    "Valid characters must be within the ranges A-Z, a-z, 0-9, '/', '_' and '-'.");
                }
            }
        }
        else
        {
            throw Exception("Invalid first character in name. "
                            "Valid characters must be within the ranges A-Z, a-z, and '/'.");
        }
    }

    /**
    * Whether the character is valid as a first character (alphanumeric or /)
    * @param c character
    * @return true if the character is a valid first character
    */
    inline kt_bool IsValidFirst(char c) { return (isalpha(c) || c == '/'); }

    /**
    * Whether the character is a valid character (alphanumeric, /, _, or -)
    * @param c character
    * @return true if the character is valid
    */
    inline kt_bool IsValid(char c)
    {
        return (isalnum(c) || c == '/' || c == '_' || c == '-');
    }

    std::string m_Name;
    std::string m_Scope;
};

////////////////////////////////////////////////////////////////////////////////////////
/**
* Abstract base class for Karto objects.
*/
class KARTO_EXPORT Object : public NonCopyable
{
public:
    Object();

    Object(const Name &rName);

    virtual ~Object();

    inline const Name &GetName() const { return m_Name; }

    /**
    * Gets the class name of this object
    * @return class name
    */
    virtual const char *GetClassName() const = 0;

    /**
    * Gets the type of this object
    * @return object type
    */
    virtual kt_objecttype GetObjectType() const = 0;

    /**
    * Gets the parameter manager of this dataset
    * @return parameter manager
    */
    virtual inline ParameterManager *GetParameterManager()
    {
        return m_pParameterManager;
    }

    /**
    * Gets the named parameter
    * @param rName name of parameter
    * @return parameter
    */
    inline AbstractParameter *GetParameter(const std::string &rName) const
    {
        return m_pParameterManager->Get(rName);
    }

    /**
    * Sets the parameter with the given name with the given value
    * @param rName name
    * @param value value
    */
    template<typename T> inline void SetParameter(const std::string &rName, T value);

    /**
    * Gets all parameters
    * @return parameters
    */
    inline const ParameterVector &GetParameters() const
    {
        return m_pParameterManager->GetParameterVector();
    }

private:
    Object(const Object &);

    const Object &operator=(const Object &);

    Name m_Name;
    ParameterManager *m_pParameterManager;
};

typedef std::vector<Object *> ObjectVector;

////////////////////////////////////////////////////////////////////////////////////////
/**
* Whether the object is a sensor
* @param pObject object
* @return whether the object is a sensor
*/
inline kt_bool IsSensor(Object *pObject)
{
    return (pObject->GetObjectType() & ObjectType_Sensor) == ObjectType_Sensor;
}

/**
* Whether the object is sensor data
* @param pObject object
* @return whether the object is sensor data
*/
inline kt_bool IsSensorData(Object *pObject)
{
    return (pObject->GetObjectType() & ObjectType_SensorData) == ObjectType_SensorData;
}

/**
* Whether the object is a laser range finder
* @param pObject object
* @return whether the object is a laser range finder
*/
inline kt_bool IsLaserRangeFinder(Object *pObject)
{
    return (pObject->GetObjectType() & ObjectType_LaserRangeFinder) == ObjectType_LaserRangeFinder;
}

/**
* Whether the object is a localized range scan
* @param pObject object
* @return whether the object is a localized range scan
*/
inline kt_bool IsLocalizedRangeScan(Object *pObject)
{
    return (pObject->GetObjectType() & ObjectType_LocalizedRangeScan) == ObjectType_LocalizedRangeScan;
}

/**
* Whether the object is a localized range scan with points
* @param pObject object
* @return whether the object is a localized range scan with points
*/
inline kt_bool IsLocalizedRangeScanWithPoints(Object *pObject)
{
    return (pObject->GetObjectType() & ObjectType_LocalizedRangeScanWithPoints) ==
           ObjectType_LocalizedRangeScanWithPoints;
}

/**
* Whether the object is a Parameters object
* @param pObject object
* @return whether the object is a Parameters object
*/
inline kt_bool IsParameters(Object *pObject)
{
    return (pObject->GetObjectType() & ObjectType_Parameters) == ObjectType_Parameters;
}

/**
* Whether the object is a DatasetInfo object
* @param pObject object
* @return whether the object is a DatasetInfo object
*/
inline kt_bool IsDatasetInfo(Object *pObject)
{
    return (pObject->GetObjectType() & ObjectType_DatasetInfo) == ObjectType_DatasetInfo;
}

////////////////////////////////////////////////////////////////////////////////////////
/**
* Abstract base class for Karto modules.
*/
class KARTO_EXPORT Module : public Object
{
public:
// @cond EXCLUDE
    KARTO_Object(Module)
// @endcond

    Module(const std::string &rName);

    virtual ~Module();

    virtual void Reset() = 0;

    virtual kt_bool Process(karto::Object *) { return false; }

private:
    Module(const Module &);

    const Module &operator=(const Module &);
};

////////////////////////////////////////////////////////////////////////////////////////
/**
* Represents a size (width, height) in 2-dimensional real space.
*/
template<typename T>
class Size2
{
public:

    Size2() : m_Width(0), m_Height(0) {}

    Size2(T width, T height) : m_Width(width), m_Height(height) {}

    Size2(const Size2 &rOther) : m_Width(rOther.m_Width), m_Height(rOther.m_Height) {}

    inline const T GetWidth() const { return m_Width; }

    inline void SetWidth(T width) { m_Width = width; }

    inline const T GetHeight() const { return m_Height; }

    inline void SetHeight(T height) { m_Height = height; }

    inline Size2 &operator=(const Size2 &rOther)
    {
        m_Width = rOther.m_Width;
        m_Height = rOther.m_Height;
        return (*this);
    }

    inline kt_bool operator==(const Size2 &rOther) const
    {
        return (m_Width == rOther.m_Width && m_Height == rOther.m_Height);
    }

    inline kt_bool operator!=(const Size2 &rOther) const
    {
        return (m_Width != rOther.m_Width || m_Height != rOther.m_Height);
    }

    friend inline std::ostream &operator<<(std::ostream &rStream, const Size2 &rSize)
    {
        rStream << "(" << rSize.m_Width << ", " << rSize.m_Height << ")";
        return rStream;
    }

private:
    T m_Width;
    T m_Height;
};  // Size2<T>

////////////////////////////////////////////////////////////////////////////////////////
template<typename T>
class Vector2
{
public:
    Vector2()
    {
        m_Values[0] = 0;
        m_Values[1] = 0;
    }

    Vector2(T x, T y)
    {
        m_Values[0] = x;
        m_Values[1] = y;
    }

    inline const T &GetX() const { return m_Values[0]; }

    inline void SetX(const T &x) { m_Values[0] = x; }

    inline const T &GetY() const { return m_Values[1]; }

    inline void SetY(const T &y) { m_Values[1] = y; }

    /**
    * Floor point operator
    * @param rOther
    */
    inline void MakeFloor(const Vector2 &rOther)
    {
        if (rOther.m_Values[0] < m_Values[0]) m_Values[0] = rOther.m_Values[0];
        if (rOther.m_Values[1] < m_Values[1]) m_Values[1] = rOther.m_Values[1];
    }

    /**
    * Ceiling point operator
    * @param rOther
    */
    inline void MakeCeil(const Vector2 &rOther)
    {
        if (rOther.m_Values[0] > m_Values[0]) m_Values[0] = rOther.m_Values[0];
        if (rOther.m_Values[1] > m_Values[1]) m_Values[1] = rOther.m_Values[1];
    }

    /**
    * Returns the square of the length of the vector
    * @return square of the length of the vector
    */
    inline kt_double SquaredLength() const
    {
        return math::Square(m_Values[0]) + math::Square(m_Values[1]);
    }

    /**
    * Returns the length of the vector (x and y).
    * @return length of the vector
    */
    inline kt_double Length() const { return sqrt(SquaredLength()); }

    /**
    * Returns the square distance to the given vector
    * @returns square distance to the given vector
    */
    inline kt_double SquaredDistance(const Vector2 &rOther) const
    {
        return (*this - rOther).SquaredLength();
    }

    /**
    * Gets the distance to the other vector2
    * @param rOther
    * @return distance to other vector2
    */
    inline kt_double Distance(const Vector2 &rOther) const
    {
        return sqrt(SquaredDistance(rOther));
    }

    inline void operator+=(const Vector2 &rOther)
    {
        m_Values[0] += rOther.m_Values[0];
        m_Values[1] += rOther.m_Values[1];
    }

    inline void operator-=(const Vector2 &rOther)
    {
        m_Values[0] -= rOther.m_Values[0];
        m_Values[1] -= rOther.m_Values[1];
    }

    inline const Vector2 operator+(const Vector2 &rOther) const
    {
        return Vector2(m_Values[0] + rOther.m_Values[0], m_Values[1] + rOther.m_Values[1]);
    }

    inline const Vector2 operator-(const Vector2 &rOther) const
    {
        return Vector2(m_Values[0] - rOther.m_Values[0], m_Values[1] - rOther.m_Values[1]);
    }

    inline void operator/=(T scalar)
    {
        m_Values[0] /= scalar;
        m_Values[1] /= scalar;
    }

    inline const Vector2 operator/(T scalar) const
    {
        return Vector2(m_Values[0] / scalar, m_Values[1] / scalar);
    }

    inline kt_double operator*(const Vector2 &rOther) const
    {
        return m_Values[0] * rOther.m_Values[0] + m_Values[1] * rOther.m_Values[1];
    }

    inline const Vector2 operator*(T scalar) const
    {
        return Vector2(m_Values[0] * scalar, m_Values[1] * scalar);
    }

    inline const Vector2 operator-(T scalar) const
    {
        return Vector2(m_Values[0] - scalar, m_Values[1] - scalar);
    }

    inline void operator*=(T scalar)
    {
        m_Values[0] *= scalar;
        m_Values[1] *= scalar;
    }

    inline kt_bool operator==(const Vector2 &rOther) const
    {
        return (m_Values[0] == rOther.m_Values[0] && m_Values[1] == rOther.m_Values[1]);
    }

    inline kt_bool operator!=(const Vector2 &rOther) const
    {
        return (m_Values[0] != rOther.m_Values[0] || m_Values[1] != rOther.m_Values[1]);
    }

    inline kt_bool operator<(const Vector2 &rOther) const
    {
        if (m_Values[0] < rOther.m_Values[0])
            return true;
        else if (m_Values[0] > rOther.m_Values[0])
            return false;
        else
            return (m_Values[1] < rOther.m_Values[1]);
    }

    friend inline std::ostream &operator<<(std::ostream &rStream, const Vector2 &rVector)
    {
        rStream << rVector.GetX() << " " << rVector.GetY();
        return rStream;
    }

    friend inline std::istream &operator>>(std::istream &rStream, const Vector2 & /*rVector*/)
    {
// Implement me!!  TODO(lucbettaieb): What the what?  Do I need to implement this?
        return rStream;
    }

private:
    T m_Values[2];
};  // Vector2<T>

typedef std::vector<Vector2<kt_double> > PointVectorDouble;
////////////////////////////////////////////////////////////////////////////////////////
/**
* Represents a vector (x, y, z) in 3-dimensional real space.
*/
template<typename T>
class Vector3
{
public:
    Vector3()
    {
        m_Values[0] = 0;
        m_Values[1] = 0;
        m_Values[2] = 0;
    }

    /**
    * Constructor initializing point location
    * @param x
    * @param y
    * @param z
    */
    Vector3(T x, T y, T z)
    {
        m_Values[0] = x;
        m_Values[1] = y;
        m_Values[2] = z;
    }

    Vector3(const Vector3 &rOther)
    {
        m_Values[0] = rOther.m_Values[0];
        m_Values[1] = rOther.m_Values[1];
        m_Values[2] = rOther.m_Values[2];
    }

    inline const T &GetX() const { return m_Values[0]; }

    inline void SetX(const T &x) { m_Values[0] = x; }

    inline const T &GetY() const { return m_Values[1]; }

    inline void SetY(const T &y) { m_Values[1] = y; }

    inline const T &GetZ() const { return m_Values[2]; }

    inline void SetZ(const T &z) { m_Values[2] = z; }

    /**
    * Floor vector operator
    * @param rOther Vector3d
    */
    inline void MakeFloor(const Vector3 &rOther)
    {
        if (rOther.m_Values[0] < m_Values[0]) m_Values[0] = rOther.m_Values[0];
        if (rOther.m_Values[1] < m_Values[1]) m_Values[1] = rOther.m_Values[1];
        if (rOther.m_Values[2] < m_Values[2]) m_Values[2] = rOther.m_Values[2];
    }

    /**
    * Ceiling vector operator
    * @param rOther Vector3d
    */
    inline void MakeCeil(const Vector3 &rOther)
    {
        if (rOther.m_Values[0] > m_Values[0]) m_Values[0] = rOther.m_Values[0];
        if (rOther.m_Values[1] > m_Values[1]) m_Values[1] = rOther.m_Values[1];
        if (rOther.m_Values[2] > m_Values[2]) m_Values[2] = rOther.m_Values[2];
    }

    /**
    * Returns the square of the length of the vector
    * @return square of the length of the vector
    */
    inline kt_double SquaredLength() const
    {
        return math::Square(m_Values[0]) + math::Square(m_Values[1]) + math::Square(m_Values[2]);
    }

    /**
    * Returns the length of the vector.
    * @return Length of the vector
    */
    inline kt_double Length() const
    {
        return sqrt(SquaredLength());
    }

    /**
    * Returns a string representation of this vector
    * @return string representation of this vector
    */
    inline std::string ToString() const
    {
        std::stringstream converter;
        converter.precision(std::numeric_limits<double>::digits10);
        converter << GetX() << " " << GetY() << " " << GetZ();
        return converter.str();
    }

public:
    inline Vector3 &operator=(const Vector3 &rOther)
    {
        m_Values[0] = rOther.m_Values[0];
        m_Values[1] = rOther.m_Values[1];
        m_Values[2] = rOther.m_Values[2];
        return *this;
    }

    inline const Vector3 operator+(const Vector3 &rOther) const
    {
        return Vector3(m_Values[0] + rOther.m_Values[0],
                       m_Values[1] + rOther.m_Values[1],
                       m_Values[2] + rOther.m_Values[2]);
    }

    inline const Vector3 operator+(kt_double scalar) const
    {
        return Vector3(m_Values[0] + scalar,
                       m_Values[1] + scalar,
                       m_Values[2] + scalar);
    }

    inline const Vector3 operator-(const Vector3 &rOther) const
    {
        return Vector3(m_Values[0] - rOther.m_Values[0],
                       m_Values[1] - rOther.m_Values[1],
                       m_Values[2] - rOther.m_Values[2]);
    }

    inline const Vector3 operator-(kt_double scalar) const
    {
        return Vector3(m_Values[0] - scalar, m_Values[1] - scalar, m_Values[2] - scalar);
    }

    inline const Vector3 operator*(T scalar) const
    {
        return Vector3(m_Values[0] * scalar, m_Values[1] * scalar, m_Values[2] * scalar);
    }

    inline kt_bool operator==(const Vector3 &rOther) const
    {
        return (m_Values[0] == rOther.m_Values[0] &&
                m_Values[1] == rOther.m_Values[1] &&
                m_Values[2] == rOther.m_Values[2]);
    }

    inline kt_bool operator!=(const Vector3 &rOther) const
    {
        return (m_Values[0] != rOther.m_Values[0] ||
                m_Values[1] != rOther.m_Values[1] ||
                m_Values[2] != rOther.m_Values[2]);
    }

    friend inline std::ostream &operator<<(std::ostream &rStream, const Vector3 &rVector)
    {
        rStream << rVector.ToString();
        return rStream;
    }

    friend inline std::istream &operator>>(std::istream &rStream, const Vector3 & /*rVector*/)
    {
// Implement me!!
        return rStream;
    }

private:
    T m_Values[3];
};  // Vector3

////////////////////////////////////////////////////////////////////////////////////////
/**
* Defines an orientation as a quaternion rotation using the positive Z axis as the zero reference.
* <BR>
* Q = w + ix + jy + kz <BR>
* w = c_1 * c_2 * c_3 - s_1 * s_2 * s_3 <BR>
* x = s_1 * s_2 * c_3 + c_1 * c_2 * s_3 <BR>
* y = s_1 * c_2 * c_3 + c_1 * s_2 * s_3 <BR>
* z = c_1 * s_2 * c_3 - s_1 * c_2 * s_3 <BR>
* where <BR>
* c_1 = cos(theta/2) <BR>
* c_2 = cos(phi/2) <BR>
* c_3 = cos(psi/2) <BR>
* s_1 = sin(theta/2) <BR>
* s_2 = sin(phi/2) <BR>
* s_3 = sin(psi/2) <BR>
* and <BR>
* theta is the angle of rotation about the Y axis measured from the Z axis. <BR>
* phi is the angle of rotation about the Z axis measured from the X axis. <BR>
* psi is the angle of rotation about the X axis measured from the Y axis. <BR>
* (All angles are right-handed.)
*/
class Quaternion
{
public:
    /**
    * Create a quaternion with default (x=0, y=0, z=0, w=1) values
    */
    inline Quaternion()
    {
        m_Values[0] = 0.0;
        m_Values[1] = 0.0;
        m_Values[2] = 0.0;
        m_Values[3] = 1.0;
    }

    inline Quaternion(kt_double x, kt_double y, kt_double z, kt_double w)
    {
        m_Values[0] = x;
        m_Values[1] = y;
        m_Values[2] = z;
        m_Values[3] = w;
    }

    inline Quaternion(const Quaternion &rQuaternion)
    {
        m_Values[0] = rQuaternion.m_Values[0];
        m_Values[1] = rQuaternion.m_Values[1];
        m_Values[2] = rQuaternion.m_Values[2];
        m_Values[3] = rQuaternion.m_Values[3];
    }

    inline kt_double GetX() const { return m_Values[0]; }

    inline void SetX(kt_double x) { m_Values[0] = x; }

    inline kt_double GetY() const { return m_Values[1]; }

    inline void SetY(kt_double y) { m_Values[1] = y; }

    inline kt_double GetZ() const { return m_Values[2]; }

    inline void SetZ(kt_double z) { m_Values[2] = z; }

    inline kt_double GetW() const { return m_Values[3]; }

    inline void SetW(kt_double w) { m_Values[3] = w; }

    /**
    * Converts this quaternion into Euler angles
    * Source: http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm
    * @param rYaw
    * @param rPitch
    * @param rRoll
    */
    void ToEulerAngles(kt_double &rYaw, kt_double &rPitch, kt_double &rRoll) const
    {
        kt_double test = m_Values[0] * m_Values[1] + m_Values[2] * m_Values[3];
        if (test > 0.499)
        {
            // singularity at north pole
            rYaw = 2 * atan2(m_Values[0], m_Values[3]);;
            rPitch = KT_PI_2;
            rRoll = 0;
        }
        else if (test < -0.499)
        {
            // singularity at south pole
            rYaw = -2 * atan2(m_Values[0], m_Values[3]);
            rPitch = -KT_PI_2;
            rRoll = 0;
        }
        else
        {
            kt_double sqx = m_Values[0] * m_Values[0];
            kt_double sqy = m_Values[1] * m_Values[1];
            kt_double sqz = m_Values[2] * m_Values[2];
            rYaw = atan2(2 * m_Values[1] * m_Values[3] - 2 * m_Values[0] * m_Values[2], 1 - 2 * sqy - 2 * sqz);
            rPitch = asin(2 * test);
            rRoll = atan2(2 * m_Values[0] * m_Values[3] - 2 * m_Values[1] * m_Values[2], 1 - 2 * sqx - 2 * sqz);
        }
    }

    /**
    * Set x,y,z,w values of the quaternion based on Euler angles.
    * Source: http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm
    * @param yaw
    * @param pitch
    * @param roll
    */
    void FromEulerAngles(kt_double yaw, kt_double pitch, kt_double roll)
    {
        kt_double angle;
        angle = yaw * 0.5;
        kt_double cYaw = cos(angle);
        kt_double sYaw = sin(angle);
        angle = pitch * 0.5;
        kt_double cPitch = cos(angle);
        kt_double sPitch = sin(angle);
        angle = roll * 0.5;
        kt_double cRoll = cos(angle);
        kt_double sRoll = sin(angle);
        m_Values[0] = sYaw * sPitch * cRoll + cYaw * cPitch * sRoll;
        m_Values[1] = sYaw * cPitch * cRoll + cYaw * sPitch * sRoll;
        m_Values[2] = cYaw * sPitch * cRoll - sYaw * cPitch * sRoll;
        m_Values[3] = cYaw * cPitch * cRoll - sYaw * sPitch * sRoll;
    }

    inline Quaternion &operator=(const Quaternion &rQuaternion)
    {
        m_Values[0] = rQuaternion.m_Values[0];
        m_Values[1] = rQuaternion.m_Values[1];
        m_Values[2] = rQuaternion.m_Values[2];
        m_Values[3] = rQuaternion.m_Values[3];
        return (*this);
    }

    inline kt_bool operator==(const Quaternion &rOther) const
    {
        return (m_Values[0] == rOther.m_Values[0] &&
                m_Values[1] == rOther.m_Values[1] &&
                m_Values[2] == rOther.m_Values[2] &&
                m_Values[3] == rOther.m_Values[3]);
    }

    inline kt_bool operator!=(const Quaternion &rOther) const
    {
        return (m_Values[0] != rOther.m_Values[0] ||
                m_Values[1] != rOther.m_Values[1] ||
                m_Values[2] != rOther.m_Values[2] ||
                m_Values[3] != rOther.m_Values[3]);
    }

    friend inline std::ostream &operator<<(std::ostream &rStream, const Quaternion &rQuaternion)
    {
        rStream << rQuaternion.m_Values[0] << " "
                << rQuaternion.m_Values[1] << " "
                << rQuaternion.m_Values[2] << " "
                << rQuaternion.m_Values[3];
        return rStream;
    }

private:
    kt_double m_Values[4];
};

////////////////////////////////////////////////////////////////////////////////////////
/**
* Stores x, y, width and height that represents the location and size of a rectangle
* (x, y) is at bottom left in mapper!
*/
template<typename T>
class Rectangle2
{
public:
    Rectangle2() {}

    Rectangle2(T x, T y, T width, T height) : m_Position(x, y), m_Size(width, height) {}

    Rectangle2(const Vector2<T> &rPosition, const Size2<T> &rSize)
            : m_Position(rPosition), m_Size(rSize) {}

    Rectangle2(const Rectangle2 &rOther)
            : m_Position(rOther.m_Position), m_Size(rOther.m_Size) {}

    inline T GetX() const { return m_Position.GetX(); }

    inline void SetX(T x) { m_Position.SetX(x); }

    inline T GetY() const { return m_Position.GetY(); }

    inline void SetY(T y) { m_Position.SetY(y); }

    inline T GetWidth() const { return m_Size.GetWidth(); }

    inline void SetWidth(T width) { m_Size.SetWidth(width); }

    inline T GetHeight() const { return m_Size.GetHeight(); }

    inline void SetHeight(T height) { m_Size.SetHeight(height); }

    inline const Vector2<T> &GetPosition() const { return m_Position; }

    inline void SetPosition(const T &rX, const T &rY) { m_Position = Vector2<T>(rX, rY); }

    inline void SetPosition(const Vector2<T> &rPosition) { m_Position = rPosition; }

    inline const Size2<T> &GetSize() const { return m_Size; }

    inline void SetSize(const Size2<T> &rSize) { m_Size = rSize; }
    
    /**
    * Gets the center of this rectangle
    * @return the center of this rectangle
    */
    inline const Vector2<T> GetCenter() const
    {
        return Vector2<T>(m_Position.GetX() + m_Size.GetWidth() * 0.5,
                          m_Position.GetY() + m_Size.GetHeight() * 0.5);
    }

    Rectangle2 &operator=(const Rectangle2 &rOther)
    {
        m_Position = rOther.m_Position;
        m_Size = rOther.m_Size;
        return *this;
    }

    inline kt_bool operator==(const Rectangle2 &rOther) const
    {
        return (m_Position == rOther.m_Position && m_Size == rOther.m_Size);
    }

    inline kt_bool operator!=(const Rectangle2 &rOther) const
    {
        return (m_Position != rOther.m_Position || m_Size != rOther.m_Size);
    }

private:
    Vector2<T> m_Position;
    Size2<T> m_Size;
};  // Rectangle2

////////////////////////////////////////////////////////////////////////////////////////
class Pose3;

/**
* Defines a position (x, y) in 2-dimensional space and heading.
*/
class Pose2
{
public:
    Pose2() : m_Heading(0.0) {}

    /**
    * Constructor initializing pose parameters
    * @param rPosition position
    * @param heading heading
    **/
    Pose2(const Vector2<kt_double> &rPosition, kt_double heading)
            : m_Position(rPosition), m_Heading(heading) {}

    /**
    * Constructor initializing pose parameters
    * @param x x-coordinate
    * @param y y-coordinate
    * @param heading heading
    **/
    Pose2(kt_double x, kt_double y, kt_double heading)
            : m_Position(x, y), m_Heading(heading) {}

    // Constructs a Pose2 object from a Pose3.
    Pose2(const Pose3 &rPose);

    Pose2(const Pose2 &rOther)
            : m_Position(rOther.m_Position), m_Heading(rOther.m_Heading) {}

    inline kt_double GetX() const { return m_Position.GetX(); }

    inline void SetX(kt_double x) { m_Position.SetX(x); }

    inline kt_double GetY() const { return m_Position.GetY(); }

    inline void SetY(kt_double y) { m_Position.SetY(y); }

    inline const Vector2<kt_double> &GetPosition() const { return m_Position; }

    inline void SetPosition(const Vector2<kt_double> &rPosition) { m_Position = rPosition; }

    inline kt_double GetHeading() const { return m_Heading; }

    inline void SetHeading(kt_double heading) { m_Heading = heading; }

    /**
    * Return the squared distance between two Pose2
    * @return squared distance
    */
    inline kt_double SquaredDistance(const Pose2 &rOther) const
    {
        return m_Position.SquaredDistance(rOther.m_Position);
    }

    inline Pose2 &operator=(const Pose2 &rOther)
    {
        m_Position = rOther.m_Position;
        m_Heading = rOther.m_Heading;
        return *this;
    }

    inline kt_bool operator==(const Pose2 &rOther) const
    {
        return (m_Position == rOther.m_Position && m_Heading == rOther.m_Heading);
    }

    inline kt_bool operator!=(const Pose2 &rOther) const
    {
        return (m_Position != rOther.m_Position || m_Heading != rOther.m_Heading);
    }

    inline void operator+=(const Pose2 &rOther)
    {
        m_Position += rOther.m_Position;
        m_Heading = math::NormalizeAngle(m_Heading + rOther.m_Heading);
    }

    inline Pose2 operator+(const Pose2 &rOther) const
    {
        return Pose2(m_Position + rOther.m_Position, math::NormalizeAngle(m_Heading + rOther.m_Heading));
    }

    inline Pose2 operator-(const Pose2 &rOther) const
    {
        return Pose2(m_Position - rOther.m_Position, math::NormalizeAngle(m_Heading - rOther.m_Heading));
    }

    friend inline std::istream &operator>>(std::istream &rStream, const Pose2 & /*rPose*/)
    {
// Implement me!!
        return rStream;
    }

    friend inline std::ostream &operator<<(std::ostream &rStream, const Pose2 &rPose)
    {
        rStream << rPose.m_Position.GetX() << " " << rPose.m_Position.GetY() << " " << rPose.m_Heading;
        return rStream;
    }

private:
    Vector2<kt_double> m_Position;

    kt_double m_Heading;
};  // Pose2

typedef std::vector<Pose2> Pose2Vector;

////////////////////////////////////////////////////////////////////////////////////////
/**
* Defines a position and orientation in 3-dimensional space.
* Karto uses a right-handed coordinate system with X, Y as the 2-D ground plane and X is forward and Y is left.
* Values in Vector3 used to define position must have units of meters.
* The value of angle when defining orientation in two dimensions must be in units of radians.
* The definition of orientation in three dimensions uses quaternions.
*/
class Pose3
{
public:
    Pose3() {}

    Pose3(const Vector3<kt_double> &rPosition) : m_Position(rPosition) {}

    Pose3(const Vector3<kt_double> &rPosition, const karto::Quaternion &rOrientation)
            : m_Position(rPosition), m_Orientation(rOrientation) {}

    Pose3(const Pose3 &rOther)
            : m_Position(rOther.m_Position), m_Orientation(rOther.m_Orientation) {}

    // Constructs a Pose3 object from a Pose2.
    Pose3(const Pose2 &rPose)
    {
        m_Position = Vector3<kt_double>(rPose.GetX(), rPose.GetY(), 0.0);
        m_Orientation.FromEulerAngles(rPose.GetHeading(), 0.0, 0.0);
    }

    inline const Vector3<kt_double> &GetPosition() const { return m_Position; }

    inline void SetPosition(const Vector3<kt_double> &rPosition) { m_Position = rPosition; }

    inline const Quaternion &GetOrientation() const
    {
        return m_Orientation;
    }

    inline void SetOrientation(const Quaternion &rOrientation)
    {
        m_Orientation = rOrientation;
    }

    inline std::string ToString()
    {
        std::stringstream converter;
        converter.precision(std::numeric_limits<double>::digits10);
        converter << GetPosition() << " " << GetOrientation();
        return converter.str();
    }

    inline Pose3 &operator=(const Pose3 &rOther)
    {
        m_Position = rOther.m_Position;
        m_Orientation = rOther.m_Orientation;
        return *this;
    }

    inline kt_bool operator==(const Pose3 &rOther) const
    {
        return (m_Position == rOther.m_Position && m_Orientation == rOther.m_Orientation);
    }

    inline kt_bool operator!=(const Pose3 &rOther) const
    {
        return (m_Position != rOther.m_Position || m_Orientation != rOther.m_Orientation);
    }

    friend inline std::ostream &operator<<(std::ostream &rStream, const Pose3 &rPose)
    {
        rStream << rPose.GetPosition() << ", " << rPose.GetOrientation();
        return rStream;
    }

    friend inline std::istream &operator>>(std::istream &rStream, const Pose3 & /*rPose*/)
    {
// Implement me!!
        return rStream;
    }

private:
    Vector3<kt_double> m_Position;
    Quaternion m_Orientation;
};  // Pose3

////////////////////////////////////////////////////////////////////////////////////////
/**
* Defines a Matrix 3 x 3 class.
*/
class Matrix3
{
public:
    Matrix3() { Clear(); }

    inline Matrix3(const Matrix3 &rOther)
    {
        memcpy(m_Matrix, rOther.m_Matrix, 9 * sizeof(kt_double));
    }

    // Sets this matrix to identity matrix
    void SetToIdentity()
    {
        memset(m_Matrix, 0, 9 * sizeof(kt_double));
        for (kt_int32s i = 0; i < 3; i++) m_Matrix[i][i] = 1.0;
    }

    void Clear() { memset(m_Matrix, 0, 9 * sizeof(kt_double)); }

    /**
    * Sets this matrix to be the rotation matrix of rotation around given axis
    * @param x x-coordinate of axis
    * @param y y-coordinate of axis
    * @param z z-coordinate of axis
    * @param radians amount of rotation
    */
    void FromAxisAngle(kt_double x, kt_double y, kt_double z, const kt_double radians)
    {
        kt_double cosRadians = cos(radians);
        kt_double sinRadians = sin(radians);
        kt_double oneMinusCos = 1.0 - cosRadians;
        kt_double xx = x * x;
        kt_double yy = y * y;
        kt_double zz = z * z;
        kt_double xyMCos = x * y * oneMinusCos;
        kt_double xzMCos = x * z * oneMinusCos;
        kt_double yzMCos = y * z * oneMinusCos;
        kt_double xSin = x * sinRadians;
        kt_double ySin = y * sinRadians;
        kt_double zSin = z * sinRadians;
        m_Matrix[0][0] = xx * oneMinusCos + cosRadians;
        m_Matrix[0][1] = xyMCos - zSin;
        m_Matrix[0][2] = xzMCos + ySin;
        m_Matrix[1][0] = xyMCos + zSin;
        m_Matrix[1][1] = yy * oneMinusCos + cosRadians;
        m_Matrix[1][2] = yzMCos - xSin;
        m_Matrix[2][0] = xzMCos - ySin;
        m_Matrix[2][1] = yzMCos + xSin;
        m_Matrix[2][2] = zz * oneMinusCos + cosRadians;
    }

    Matrix3 Transpose() const
    {
        Matrix3 transpose;
        for (kt_int32u row = 0; row < 3; row++)
        {
            for (kt_int32u col = 0; col < 3; col++)
            {
                transpose.m_Matrix[row][col] = m_Matrix[col][row];
            }
        }
        return transpose;
    }

    // Returns the inverse of the matrix
    Matrix3 Inverse() const
    {
        Matrix3 kInverse = *this;
        kt_bool haveInverse = InverseFast(kInverse, 1e-14);
        if (!haveInverse) assert(false);
        return kInverse;
    }

    /**
    * Internal helper method for inverse matrix calculation
    * This code is lifted from the OgreMatrix3 class!!
    */
    kt_bool InverseFast(Matrix3 &rkInverse, kt_double fTolerance = KT_TOLERANCE) const
    {
        // Invert a 3x3 using cofactors.
        // This is about 8 times faster than the Numerical Recipes code which uses Gaussian elimination.
        rkInverse.m_Matrix[0][0] = m_Matrix[1][1] * m_Matrix[2][2] - m_Matrix[1][2] * m_Matrix[2][1];
        rkInverse.m_Matrix[0][1] = m_Matrix[0][2] * m_Matrix[2][1] - m_Matrix[0][1] * m_Matrix[2][2];
        rkInverse.m_Matrix[0][2] = m_Matrix[0][1] * m_Matrix[1][2] - m_Matrix[0][2] * m_Matrix[1][1];
        rkInverse.m_Matrix[1][0] = m_Matrix[1][2] * m_Matrix[2][0] - m_Matrix[1][0] * m_Matrix[2][2];
        rkInverse.m_Matrix[1][1] = m_Matrix[0][0] * m_Matrix[2][2] - m_Matrix[0][2] * m_Matrix[2][0];
        rkInverse.m_Matrix[1][2] = m_Matrix[0][2] * m_Matrix[1][0] - m_Matrix[0][0] * m_Matrix[1][2];
        rkInverse.m_Matrix[2][0] = m_Matrix[1][0] * m_Matrix[2][1] - m_Matrix[1][1] * m_Matrix[2][0];
        rkInverse.m_Matrix[2][1] = m_Matrix[0][1] * m_Matrix[2][0] - m_Matrix[0][0] * m_Matrix[2][1];
        rkInverse.m_Matrix[2][2] = m_Matrix[0][0] * m_Matrix[1][1] - m_Matrix[0][1] * m_Matrix[1][0];

        kt_double fDet = m_Matrix[0][0] * rkInverse.m_Matrix[0][0] +
                         m_Matrix[0][1] * rkInverse.m_Matrix[1][0] +
                         m_Matrix[0][2] * rkInverse.m_Matrix[2][0];
        if (fabs(fDet) <= fTolerance) return false;
        kt_double fInvDet = 1.0 / fDet;
        for (size_t row = 0; row < 3; row++)
        {
            for (size_t col = 0; col < 3; col++)
            {
                rkInverse.m_Matrix[row][col] *= fInvDet;
            }
        }
        return true;
    }

    /**
    * Returns a string representation of this matrix
    * @return string representation of this matrix
    */
    inline std::string ToString() const
    {
        std::stringstream converter;
        converter.precision(std::numeric_limits<double>::digits10);
        for (int row = 0; row < 3; row++)
        {
            for (int col = 0; col < 3; col++)
            {
                converter << m_Matrix[row][col] << " ";
            }
        }
        return converter.str();
    }

public:
    inline Matrix3 &operator=(const Matrix3 &rOther)
    {
        memcpy(m_Matrix, rOther.m_Matrix, 9 * sizeof(kt_double));
        return *this;
    }

    /**
    * Matrix element access, allows use of construct mat(r, c)
    * @param row
    * @param column
    * @return reference to mat(r,c)
    */
    inline kt_double &operator()(kt_int32u row, kt_int32u column)
    {
        return m_Matrix[row][column];
    }

    /**
    * Read-only matrix element access, allows use of construct mat(r, c)
    * @param row
    * @param column
    * @return mat(r,c)
    */
    inline kt_double operator()(kt_int32u row, kt_int32u column) const
    {
        return m_Matrix[row][column];
    }

    /**
    * Binary Matrix3 multiplication.
    * @param rOther
    * @return Matrix3 product
    */
    Matrix3 operator*(const Matrix3 &rOther) const
    {
        Matrix3 product;
        for (size_t row = 0; row < 3; row++)
        {
            for (size_t col = 0; col < 3; col++)
            {
                product.m_Matrix[row][col] = m_Matrix[row][0] * rOther.m_Matrix[0][col] +
                                             m_Matrix[row][1] * rOther.m_Matrix[1][col] +
                                             m_Matrix[row][2] * rOther.m_Matrix[2][col];
            }
        }

        return product;
    }

    /**
    * Matrix3 and Pose2 multiplication - matrix * pose [3x3 * 3x1 = 3x1]
    * @param rPose2
    * @return Pose2 product
    */
    inline Pose2 operator*(const Pose2 &rPose2) const
    {
        Pose2 pose2;
        pose2.SetX(m_Matrix[0][0] * rPose2.GetX() +
                   m_Matrix[0][1] * rPose2.GetY() +
                   m_Matrix[0][2] * rPose2.GetHeading());
        pose2.SetY(m_Matrix[1][0] * rPose2.GetX() +
                   m_Matrix[1][1] * rPose2.GetY() +
                   m_Matrix[1][2] * rPose2.GetHeading());
        pose2.SetHeading(m_Matrix[2][0] * rPose2.GetX() +
                         m_Matrix[2][1] * rPose2.GetY() +
                         m_Matrix[2][2] * rPose2.GetHeading());
        return pose2;
    }

    inline void operator+=(const Matrix3 &rkMatrix)
    {
        for (kt_int32u row = 0; row < 3; row++)
        {
            for (kt_int32u col = 0; col < 3; col++)
            {
                m_Matrix[row][col] += rkMatrix.m_Matrix[row][col];
            }
        }
    }

    friend inline std::ostream &operator<<(std::ostream &rStream, const Matrix3 &rMatrix)
    {
        rStream << rMatrix.ToString();
        return rStream;
    }

private:
    kt_double m_Matrix[3][3];
};  // Matrix3

////////////////////////////////////////////////////////////////////////////////////////
/**
* Defines a general Matrix class.
*/
class Matrix
{
public:
    /**
    * Constructs a matrix of size rows x columns
    */
    Matrix(kt_int32u rows, kt_int32u columns) : m_Rows(rows), m_Columns(columns), m_pData(NULL)
    {
        Allocate();
        Clear();
    }

    virtual ~Matrix() { delete[] m_pData; }

    void Clear()
    {
        if (m_pData != NULL)
        {
            memset(m_pData, 0, sizeof(kt_double) * m_Rows * m_Columns);
        }
    }

    inline kt_int32u GetRows() const { return m_Rows; }

    inline kt_int32u GetColumns() const { return m_Columns; }

    /**
    * Returns a reference to the entry at (row,column)
    * @param row
    * @param column
    * @return reference to entry at (row,column)
    */
    inline kt_double &operator()(kt_int32u row, kt_int32u column)
    {
        RangeCheck(row, column);
        return m_pData[row + column * m_Rows];
    }

    /**
    * Returns a const reference to the entry at (row,column)
    * @param row
    * @param column
    * @return const reference to entry at (row,column)
    */
    inline const kt_double &operator()(kt_int32u row, kt_int32u column) const
    {
        RangeCheck(row, column);
        return m_pData[row + column * m_Rows];
    }

private:
    /**
    * Allocate space for the matrix
    */
    void Allocate()
    {
        try
        {
            delete[] m_pData;
            m_pData = new kt_double[m_Rows * m_Columns];
        }
        catch (const std::bad_alloc &ex)
        {
            throw Exception("Matrix allocation error");
        }
        if (m_pData == NULL)
        {
            throw Exception("Matrix allocation error");
        }
    }

    /**
    * Checks if (row,column) is a valid entry into the matrix
    * @param row
    * @param column
    */
    inline void RangeCheck(kt_int32u row, kt_int32u column) const
    {
        if (math::IsUpTo(row, m_Rows) == 0)
        {
            throw Exception("Matrix - RangeCheck ERROR!!!!");
        }
        if (math::IsUpTo(column, m_Columns) == 0)
        {
            throw Exception("Matrix - RangeCheck ERROR!!!!");
        }
    }

private:
    kt_int32u m_Rows;
    kt_int32u m_Columns;
    kt_double *m_pData;
};  // Matrix

////////////////////////////////////////////////////////////////////////////////////////
/**
* Defines a bounding box in 2-dimensional real space.
*/
class BoundingBox2
{
public:
    BoundingBox2()
            : m_Minimum(999999999999999999.99999, 999999999999999999.99999),
              m_Maximum(-999999999999999999.99999, -999999999999999999.99999) {}

    inline const Vector2<kt_double> &GetMinimum() const { return m_Minimum; }

    inline void SetMinimum(const Vector2<kt_double> &mMinimum) { m_Minimum = mMinimum; }

    inline const Vector2<kt_double> &GetMaximum() const { return m_Maximum; }

    inline void SetMaximum(const Vector2<kt_double> &rMaximum) { m_Maximum = rMaximum; }

    inline Size2<kt_double> GetSize() const
    {
        Vector2<kt_double> size = m_Maximum - m_Minimum;
        return Size2<kt_double>(size.GetX(), size.GetY());
    }

    inline void Add(const Vector2<kt_double> &rPoint)
    {
        m_Minimum.MakeFloor(rPoint);
        m_Maximum.MakeCeil(rPoint);
    }

    inline void Add(const BoundingBox2 &rBoundingBox)
    {
        Add(rBoundingBox.GetMinimum());
        Add(rBoundingBox.GetMaximum());
    }

    /**
    * Whether the given point is in the bounds of this box
    * @param rPoint
    * @return in bounds?
    */
    inline kt_bool IsInBounds(const Vector2<kt_double> &rPoint) const
    {
        return (math::InRange(rPoint.GetX(), m_Minimum.GetX(), m_Maximum.GetX()) &&
                math::InRange(rPoint.GetY(), m_Minimum.GetY(), m_Maximum.GetY()));
    }

private:
    Vector2<kt_double> m_Minimum;
    Vector2<kt_double> m_Maximum;
};  // BoundingBox2

////////////////////////////////////////////////////////////////////////////////////////
/**
* Implementation of a Pose2 transform
*/
class Transform
{
public:
    Transform(const Pose2 &rPose) { SetTransform(Pose2(), rPose); }

    /**
    * Constructs a transformation from the first pose to the second pose
    * @param rPose1 first pose
    * @param rPose2 second pose
    */
    Transform(const Pose2 &rPose1, const Pose2 &rPose2) { SetTransform(rPose1, rPose2); }

    /**
    * Transforms the pose according to this transform
    * @param rSourcePose pose to transform from
    * @return transformed pose
    */
    inline Pose2 TransformPose(const Pose2 &rSourcePose)
    {
        Pose2 newPosition = m_Transform + m_Rotation * rSourcePose;
        kt_double angle = math::NormalizeAngle(rSourcePose.GetHeading() + m_Transform.GetHeading());
        return Pose2(newPosition.GetPosition(), angle);
    }

    /**
    * Inverse transformation of the pose according to this transform
    * @param rSourcePose pose to transform from
    * @return transformed pose
    */
    inline Pose2 InverseTransformPose(const Pose2 &rSourcePose)
    {
        Pose2 newPosition = m_InverseRotation * (rSourcePose - m_Transform);
        kt_double angle = math::NormalizeAngle(rSourcePose.GetHeading() - m_Transform.GetHeading());
        // components of transform
        return Pose2(newPosition.GetPosition(), angle);
    }

private:
    /**
    * Sets this to be the transformation from the first pose to the second pose
    * @param rPose1 first pose
    * @param rPose2 second pose
    */
    void SetTransform(const Pose2 &rPose1, const Pose2 &rPose2)
    {
        if (rPose1 == rPose2)
        {
            m_Rotation.SetToIdentity();
            m_InverseRotation.SetToIdentity();
            m_Transform = Pose2();
            return;
        }
        // heading transformation
        m_Rotation.FromAxisAngle(0, 0, 1, rPose2.GetHeading() - rPose1.GetHeading());
        m_InverseRotation.FromAxisAngle(0, 0, 1, rPose1.GetHeading() - rPose2.GetHeading());
        // position transformation
        Pose2 newPosition;
        if (rPose1.GetX() != 0.0 || rPose1.GetY() != 0.0)
        {
            newPosition = rPose2 - m_Rotation * rPose1;
        }
        else newPosition = rPose2;
        m_Transform = Pose2(newPosition.GetPosition(), rPose2.GetHeading() - rPose1.GetHeading());
    }

    // pose transformation
    Pose2 m_Transform;
    Matrix3 m_Rotation;
    Matrix3 m_InverseRotation;
};  // Transform

////////////////////////////////////////////////////////////////////////////////////////
/**
* Enumerated type for valid LaserRangeFinder types
*/
typedef enum
{
    LaserRangeFinder_Custom = 0,
    LaserRangeFinder_Sick_LMS100 = 1,
    LaserRangeFinder_Sick_LMS200 = 2,
    LaserRangeFinder_Sick_LMS291 = 3,
    LaserRangeFinder_Hokuyo_UTM_30LX = 4,
    LaserRangeFinder_Hokuyo_URG_04LX = 5
} LaserRangeFinderType;

////////////////////////////////////////////////////////////////////////////////////////
/**
* Abstract base class for Parameters
*/
class AbstractParameter
{
public:
    AbstractParameter(const std::string &rName, ParameterManager *pParameterManger = NULL)
            : m_Name(rName)
    {
        // if parameter manager is provided add myself to it!
        if (pParameterManger != NULL) pParameterManger->Add(this);
    }

    AbstractParameter(const std::string &rName, const std::string &rDescription,
                      ParameterManager *pParameterManger = NULL)
            : m_Name(rName), m_Description(rDescription)
    {
        // if parameter manager is provided add myself to it!
        if (pParameterManger != NULL) pParameterManger->Add(this);
    }

    virtual ~AbstractParameter() {}

    inline const std::string &GetName() const { return m_Name; }

    inline const std::string &GetDescription() const { return m_Description; }

    virtual const std::string GetValueAsString() const = 0;

    virtual void SetValueFromString(const std::string &rStringValue) = 0;

    virtual AbstractParameter *Clone() = 0;

    /**
    * Write this parameter onto output stream
    * @param rStream output stream
    * @param rParameter
    */
    friend std::ostream &operator<<(std::ostream &rStream, const AbstractParameter &rParameter)
    {
        rStream.precision(6);
        rStream.flags(std::ios::fixed);
        rStream << rParameter.GetName() << " = " << rParameter.GetValueAsString();
        return rStream;
    }

private:
    std::string m_Name;
    std::string m_Description;
};  // AbstractParameter

////////////////////////////////////////////////////////////////////////////////////////
/**
* Parameter class
*/
template<typename T>
class Parameter : public AbstractParameter
{
public:
    /**
    * Parameter with given name and value
    * @param rName
    * @param value
    * @param pParameterManger
    */
    Parameter(const std::string &rName, T value, ParameterManager *pParameterManger = NULL)
            : AbstractParameter(rName, pParameterManger), m_Value(value) {}

    /**
    * Parameter with given name, description and value
    * @param rName
    * @param rDescription
    * @param value
    * @param pParameterManger
    */
    Parameter(const std::string &rName,
              const std::string &rDescription,
              T value,
              ParameterManager *pParameterManger = NULL)
            : AbstractParameter(rName, rDescription, pParameterManger), m_Value(value) {}

    virtual ~Parameter() {}

    inline const T &GetValue() const { return m_Value; }

    inline void SetValue(const T &rValue) { m_Value = rValue; }

    virtual const std::string GetValueAsString() const
    {
        std::stringstream converter;
        converter << m_Value;
        return converter.str();
    }

    virtual void SetValueFromString(const std::string &rStringValue)
    {
        std::stringstream converter;
        converter.str(rStringValue);
        converter >> m_Value;
    }

    virtual Parameter *Clone() { return new Parameter(GetName(), GetDescription(), GetValue()); }

    Parameter &operator=(const Parameter &rOther)
    {
        m_Value = rOther.m_Value;
        return *this;
    }

    T operator=(T value)
    {
        m_Value = value;
        return m_Value;
    }

protected:
    T m_Value;
};  // Parameter

template<>
inline void Parameter<kt_double>::SetValueFromString(const std::string &rStringValue)
{
    int precision = std::numeric_limits<double>::digits10;
    std::stringstream converter;
    converter.precision(precision);
    converter.str(rStringValue);
    m_Value = 0.0;
    converter >> m_Value;
}

template<>
inline const std::string Parameter<kt_double>::GetValueAsString() const
{
    std::stringstream converter;
    converter.precision(std::numeric_limits<double>::digits10);
    converter << m_Value;
    return converter.str();
}

template<>
inline void Parameter<kt_bool>::SetValueFromString(const std::string &rStringValue)
{
    if (rStringValue == "true" || rStringValue == "TRUE") m_Value = true;
    else m_Value = false;
}

template<>
inline const std::string Parameter<kt_bool>::GetValueAsString() const
{
    if (m_Value != 0) return "true";
    return "false";
}

////////////////////////////////////////////////////////////////////////////////////////
class ParameterEnum : public Parameter<kt_int32s>
{
    typedef std::map<std::string, kt_int32s> EnumMap;
public:
    /**
    * Construct a Parameter object with name and value
    * @param rName parameter name
    * @param value of parameter
    * @param pParameterManger
    */
    ParameterEnum(const std::string &rName, kt_int32s value, ParameterManager *pParameterManger = NULL)
            : Parameter<kt_int32s>(rName, value, pParameterManger) {}

    virtual ~ParameterEnum() {}

    /**
    * Return a clone of this instance
    * @return clone
    */
    virtual Parameter<kt_int32s> *Clone()
    {
        ParameterEnum *pEnum = new ParameterEnum(GetName(), GetValue());
        pEnum->m_EnumDefines = m_EnumDefines;
        return pEnum;
    }

    virtual void SetValueFromString(const std::string &rStringValue)
    {
        if (m_EnumDefines.find(rStringValue) != m_EnumDefines.end())
        {
            m_Value = m_EnumDefines[rStringValue];
        }
        else
        {
            std::string validValues;
            const_forEach(EnumMap, &m_EnumDefines)
            {
                validValues += iter->first + ", ";
            }
            throw Exception("Unable to set enum: " + rStringValue + ". Valid values are: " + validValues);
        }
    }

    virtual const std::string GetValueAsString() const
    {
        const_forEach(EnumMap, &m_EnumDefines)
        {
            if (iter->second == m_Value) return iter->first;
        }
        throw Exception("Unable to lookup enum");
    }

    /**
    * Defines the enum with the given name as having the given value
    * @param value
    * @param rName
    */
    void DefineEnumValue(kt_int32s value, const std::string &rName)
    {
        if (m_EnumDefines.find(rName) == m_EnumDefines.end()) m_EnumDefines[rName] = value;
        else
        {
            std::cerr << "Overriding enum value: " << m_EnumDefines[rName] << " with " << value << std::endl;
            m_EnumDefines[rName] = value;
            assert(false);
        }
    }

    ParameterEnum &operator=(const ParameterEnum &rOther)
    {
        SetValue(rOther.GetValue());
        return *this;
    }

    kt_int32s operator=(kt_int32s value)
    {
        SetValue(value);
        return m_Value;
    }

private:
    EnumMap m_EnumDefines;
};  // ParameterEnum

////////////////////////////////////////////////////////////////////////////////////////
/**
* Set of parameters
*/
class Parameters : public Object
{
public:
    KARTO_Object(Parameters)
/**
* Parameters
* @param rName
*/
    Parameters(const std::string &rName) : Object(rName) {}

    virtual ~Parameters() {}

private:
    Parameters(const Parameters &);

    const Parameters &operator=(const Parameters &);
};  // Parameters

////////////////////////////////////////////////////////////////////////////////////////
class SensorData;

/**
* Abstract Sensor base class
*/
class KARTO_EXPORT Sensor : public Object
{
protected:
    Sensor(const Name &rName);

public:
// @cond EXCLUDE
    KARTO_Object(Sensor)
// @endcond

    virtual ~Sensor();

    inline const Pose2 &GetOffsetPose() const
    {
        return m_pOffsetPose->GetValue();
    }

    inline void SetOffsetPose(const Pose2 &rPose)
    {
        m_pOffsetPose->SetValue(rPose);
    }

    virtual kt_bool Validate() = 0;

    virtual kt_bool Validate(SensorData *pSensorData) = 0;

private:
    Sensor(const Sensor &);

    const Sensor &operator=(const Sensor &);

    Parameter<Pose2> *m_pOffsetPose;
};  // Sensor

/**
* Type declaration of Sensor vector
*/
typedef std::vector<Sensor *> SensorVector;

////////////////////////////////////////////////////////////////////////////////////////
/**
* Type declaration of <Name, Sensor*> map
*/
typedef std::map<Name, Sensor *> SensorManagerMap;

/**
* Manages sensors
*/
class KARTO_EXPORT SensorManager
{
public:
    SensorManager() {}

    virtual ~SensorManager() {}

    static SensorManager *GetInstance();

    /**
    * Registers a sensor by it's name. The Sensor name must be unique, if not sensor is not registered
    * unless override is set to true
    * @param pSensor sensor to register
    * @param override
    * @return true if sensor is registered with SensorManager, false if Sensor name is not unique
    */
    void RegisterSensor(Sensor *pSensor, kt_bool override = false)
    {
        Validate(pSensor);
        if ((m_Sensors.find(pSensor->GetName()) != m_Sensors.end()) && !override)
        {
            throw Exception("Cannot register sensor: already registered: [" +
                            pSensor->GetName().ToString() +
                            "] (Consider setting 'override' to true)");
        }
        std::cout << "Registering sensor: [" << pSensor->GetName().ToString() << "]" << std::endl;
        m_Sensors[pSensor->GetName()] = pSensor;
    }

    /**
    * Unregisters the given sensor
    * @param pSensor sensor to unregister
    */
    void UnregisterSensor(Sensor *pSensor)
    {
        Validate(pSensor);
        if (m_Sensors.find(pSensor->GetName()) != m_Sensors.end())
        {
            std::cout << "Unregistering sensor: " << pSensor->GetName().ToString() << std::endl;
            m_Sensors.erase(pSensor->GetName());
        }
        else throw Exception("Cannot unregister sensor: not registered: [" + pSensor->GetName().ToString() + "]");
    }

    /**
    * Gets the sensor with the given name
    * @param rName name of sensor
    * @return sensor
    */
    Sensor *GetSensorByName(const Name &rName)
    {
        if (m_Sensors.find(rName) != m_Sensors.end())
        {
            return m_Sensors[rName];
        }
        throw Exception(
                "Sensor not registered: [" + rName.ToString() + "] (Did you add the sensor to the Dataset?)");
    }

    template<class T>
    T *GetSensorByName(const Name &rName)
    {
        Sensor *pSensor = GetSensorByName(rName);
        return dynamic_cast<T *>(pSensor);
    }

    SensorVector GetAllSensors()
    {
        SensorVector sensors;
        forEach(SensorManagerMap, &m_Sensors)
        {
            sensors.push_back(iter->second);
        }
        return sensors;
    }

protected:
    /**
    * Checks that given sensor is not NULL and has non-empty name
    * @param pSensor sensor to validate
    */
    static void Validate(Sensor *pSensor)
    {
        if (pSensor == NULL) throw Exception("Invalid sensor:  NULL");
        else if (pSensor->GetName().ToString() == "")
        {
            throw Exception("Invalid sensor:  nameless");
        }
    }

    /**
    * Sensor map
    */
    SensorManagerMap m_Sensors;
};

////////////////////////////////////////////////////////////////////////////////////////
/**
* Sensor that provides pose information relative to world coordinates.
*
* The user can set the offset pose of the drive sensor.  If no value is provided by the user the default is no offset,
* i.e, the sensor is initially at the world origin, oriented along the positive z axis.
*/
class Drive : public Sensor
{
public:
// @cond EXCLUDE
    KARTO_Object(Drive)
// @endcond

    Drive(const std::string &rName) : Sensor(rName) {}

    virtual ~Drive() {}

    virtual kt_bool Validate() { return true; }

    virtual kt_bool Validate(SensorData *pSensorData)
    {
        if (pSensorData == NULL) return false;
        return true;
    }

private:
    Drive(const Drive &);
    const Drive &operator=(const Drive &);
};  // class Drive

////////////////////////////////////////////////////////////////////////////////////////
class LocalizedRangeScan;

class CoordinateConverter;

/**
* The LaserRangeFinder defines a laser sensor that provides the pose offset position of a localized range scan relative to the robot.
* The user can set an offset pose for the sensor relative to the robot coordinate system. If no value is provided
* by the user, the sensor is set to be at the origin of the robot coordinate system.
* The LaserRangeFinder contains parameters for physical laser sensor used by the mapper for scan matching
* Also contains information about the maximum range of the sensor and provides a threshold
* for limiting the range of readings.
* The optimal value for the range threshold depends on the angular resolution of the scan and
* the desired map resolution.  RangeThreshold should be set as large as possible while still
* providing "solid" coverage between consecutive range readings.  The diagram below illustrates
* the relationship between map resolution and the range threshold.
*/
class KARTO_EXPORT LaserRangeFinder : public Sensor
{
public:
// @cond EXCLUDE
    KARTO_Object(LaserRangeFinder)
// @endcond
    virtual ~LaserRangeFinder() {}

    inline kt_double GetMinimumRange() const
    {
        return m_pMinimumRange->GetValue();
    }

    inline void SetMinimumRange(kt_double minimumRange)
    {
        m_pMinimumRange->SetValue(minimumRange);
        SetRangeThreshold(GetRangeThreshold());
    }

    inline kt_double GetMaximumRange() const
    {
        return m_pMaximumRange->GetValue();
    }

    inline void SetMaximumRange(kt_double maximumRange)
    {
        m_pMaximumRange->SetValue(maximumRange);
        SetRangeThreshold(GetRangeThreshold());
    }

    inline kt_double GetRangeThreshold() const
    {
        return m_pRangeThreshold->GetValue();
    }

    inline void SetRangeThreshold(kt_double rangeThreshold)
    {
        // make sure rangeThreshold is within laser range finder range
        m_pRangeThreshold->SetValue(math::Clip(rangeThreshold, GetMinimumRange(), GetMaximumRange()));
        if (math::DoubleEqual(GetRangeThreshold(), rangeThreshold) == false)
        {
            std::cout << "Info: clipped range threshold to be within minimum and maximum range!" << std::endl;
        }
    }

    inline kt_double GetMinimumAngle() const
    {
        return m_pMinimumAngle->GetValue();
    }

    inline void SetMinimumAngle(kt_double minimumAngle)
    {
        m_pMinimumAngle->SetValue(minimumAngle);
        Update();
    }

    inline kt_double GetMaximumAngle() const
    {
        return m_pMaximumAngle->GetValue();
    }

    inline void SetMaximumAngle(kt_double maximumAngle)
    {
        m_pMaximumAngle->SetValue(maximumAngle);
        Update();
    }

    inline kt_double GetAngularResolution() const
    {
        return m_pAngularResolution->GetValue();
    }

    inline void SetAngularResolution(kt_double angularResolution)
    {
        if (m_pType->GetValue() == LaserRangeFinder_Custom)
        {
            m_pAngularResolution->SetValue(angularResolution);
        }
        else if (m_pType->GetValue() == LaserRangeFinder_Sick_LMS100)
        {
            if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.25)))
            {
                m_pAngularResolution->SetValue(math::DegreesToRadians(0.25));
            }
            else if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.50)))
            {
                m_pAngularResolution->SetValue(math::DegreesToRadians(0.50));
            }
            else
            {
                std::stringstream stream;
                stream << "Invalid resolution for Sick LMS100:  ";
                stream << angularResolution;
                throw Exception(stream.str());
            }
        }
        else if (m_pType->GetValue() == LaserRangeFinder_Sick_LMS200 ||
                 m_pType->GetValue() == LaserRangeFinder_Sick_LMS291)
        {
            if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.25)))
            {
                m_pAngularResolution->SetValue(math::DegreesToRadians(0.25));
            }
            else if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.50)))
            {
                m_pAngularResolution->SetValue(math::DegreesToRadians(0.50));
            }
            else if (math::DoubleEqual(angularResolution, math::DegreesToRadians(1.00)))
            {
                m_pAngularResolution->SetValue(math::DegreesToRadians(1.00));
            }
            else
            {
                std::stringstream stream;
                stream << "Invalid resolution for Sick LMS291:  ";
                stream << angularResolution;
                throw Exception(stream.str());
            }
        }
        else if (m_pType->GetValue() == LaserRangeFinder_Hokuyo_UTM_30LX)
        {
            if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.25)))
            {
                m_pAngularResolution->SetValue(math::DegreesToRadians(0.25));
            }
            else
            {
                std::stringstream stream;
                stream << "Invalid resolution for Hokuyo_UTM_30LX:  ";
                stream << angularResolution;
                throw Exception(stream.str());
            }
        }
        else if (m_pType->GetValue() == LaserRangeFinder_Hokuyo_URG_04LX)
        {
            if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.352)))
            {
                m_pAngularResolution->SetValue(math::DegreesToRadians(0.352));
            }
            else
            {
                std::stringstream stream;
                stream << "Invalid resolution for Hokuyo_URG_04LX:  ";
                stream << angularResolution;
                throw Exception(stream.str());
            }
        }
        else throw Exception("Can't set angular resolution, please create a LaserRangeFinder of type Custom");
        Update();
    }

    inline kt_int32s GetType()
    {
        return m_pType->GetValue();
    }

    inline kt_int32u GetNumberOfRangeReadings() const
    {
        return m_NumberOfRangeReadings;
    }

    virtual kt_bool Validate()
    {
        Update();

        if (math::InRange(GetRangeThreshold(), GetMinimumRange(), GetMaximumRange()) == false)
        {
            std::cout << "Please set range threshold to a value between ["
                      << GetMinimumRange() << ";" << GetMaximumRange() << "]" << std::endl;
            return false;
        }

        return true;
    }

    virtual kt_bool Validate(SensorData *pSensorData);

    const PointVectorDouble GetPointReadings(LocalizedRangeScan *pLocalizedRangeScan,
                                             CoordinateConverter *pCoordinateConverter,
                                             kt_bool ignoreThresholdPoints = true,
                                             kt_bool flipY = false) const;
    /**
    * Create a laser range finder of the given type and ID
    * @param type
    * @param rName name of sensor - if no name is specified default name will be assigned
    * @return laser range finder
    */
    static LaserRangeFinder *CreateLaserRangeFinder(LaserRangeFinderType type, const Name &rName)
    {
        LaserRangeFinder *pLrf = NULL;
        switch (type)
        {
            // see http://www.hizook.com/files/publications/SICK_LMS100.pdf
            // set range threshold to 18m
            case LaserRangeFinder_Sick_LMS100:
            {
                pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Sick LMS 100"));
                // Sensing range is 18 meters (at 10% reflectivity, max range of 20 meters), with an error of about 20mm
                pLrf->m_pMinimumRange->SetValue(0.0);
                pLrf->m_pMaximumRange->SetValue(20.0);
                // 270 degree range, 50 Hz
                pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-135));
                pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(135));
                // 0.25 degree angular resolution
                pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.25));
                pLrf->m_NumberOfRangeReadings = 1081;
            }
                break;
                // see http://www.hizook.com/files/publications/SICK_LMS200-291_Tech_Info.pdf
                // set range threshold to 10m
            case LaserRangeFinder_Sick_LMS200:
            {
                pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Sick LMS 200"));
                // Sensing range is 80 meters
                pLrf->m_pMinimumRange->SetValue(0.0);
                pLrf->m_pMaximumRange->SetValue(80.0);
                // 180 degree range, 75 Hz
                pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-90));
                pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(90));
                // 0.5 degree angular resolution
                pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.5));
                pLrf->m_NumberOfRangeReadings = 361;
            }
                break;

                // see http://www.hizook.com/files/publications/SICK_LMS200-291_Tech_Info.pdf
                // set range threshold to 30m
            case LaserRangeFinder_Sick_LMS291:
            {
                pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Sick LMS 291"));
                // Sensing range is 80 meters
                pLrf->m_pMinimumRange->SetValue(0.0);
                pLrf->m_pMaximumRange->SetValue(80.0);
                // 180 degree range, 75 Hz
                pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-90));
                pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(90));
                // 0.5 degree angular resolution
                pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.5));
                pLrf->m_NumberOfRangeReadings = 361;
            }
                break;

                // see http://www.hizook.com/files/publications/Hokuyo_UTM_LaserRangeFinder_LIDAR.pdf
                // set range threshold to 30m
            case LaserRangeFinder_Hokuyo_UTM_30LX:
            {
                pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Hokuyo UTM-30LX"));
                // Sensing range is 30 meters
                pLrf->m_pMinimumRange->SetValue(0.1);
                pLrf->m_pMaximumRange->SetValue(30.0);
                // 270 degree range, 40 Hz
                pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-135));
                pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(135));
                // 0.25 degree angular resolution
                pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.25));
                pLrf->m_NumberOfRangeReadings = 1081;
            }
                break;

                // see http://www.hizook.com/files/publications/HokuyoURG_Datasheet.pdf
                // set range threshold to 3.5m
            case LaserRangeFinder_Hokuyo_URG_04LX:
            {
                pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Hokuyo URG-04LX"));
                // Sensing range is 4 meters. It has detection problems when
                // scanning absorptive surfaces (such as black trimming).
                pLrf->m_pMinimumRange->SetValue(0.02);
                pLrf->m_pMaximumRange->SetValue(4.0);
                // 240 degree range, 10 Hz
                pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-120));
                pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(120));
                // 0.352 degree angular resolution
                pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.352));
                pLrf->m_NumberOfRangeReadings = 751;
            }
                break;

            case LaserRangeFinder_Custom:
            {
                pLrf = new LaserRangeFinder(
                        (rName.GetName() != "") ? rName : Name("User-Defined LaserRangeFinder"));
                // Sensing range is 80 meters.
                pLrf->m_pMinimumRange->SetValue(0.0);
                pLrf->m_pMaximumRange->SetValue(80.0);
                // 180 degree range
                pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-90));
                pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(90));
                // 1.0 degree angular resolution
                pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(1.0));
                pLrf->m_NumberOfRangeReadings = 181;
            }
                break;
        }
        if (pLrf != NULL)
        {
            pLrf->m_pType->SetValue(type);
            Pose2 defaultOffset;
            pLrf->SetOffsetPose(defaultOffset);
        }
        return pLrf;
    }

private:
    LaserRangeFinder(const Name &rName) : Sensor(rName), m_NumberOfRangeReadings(0)
    {
        m_pMinimumRange = new Parameter<kt_double>("MinimumRange", 0.0, GetParameterManager());
        m_pMaximumRange = new Parameter<kt_double>("MaximumRange", 80.0, GetParameterManager());
        m_pMinimumAngle = new Parameter<kt_double>("MinimumAngle", -KT_PI_2, GetParameterManager());
        m_pMaximumAngle = new Parameter<kt_double>("MaximumAngle", KT_PI_2, GetParameterManager());
        m_pAngularResolution = new Parameter<kt_double>("AngularResolution",
                                                        math::DegreesToRadians(1), GetParameterManager());
        m_pRangeThreshold = new Parameter<kt_double>("RangeThreshold", 12.0, GetParameterManager());
        m_pType = new ParameterEnum("Type", LaserRangeFinder_Custom, GetParameterManager());
        m_pType->DefineEnumValue(LaserRangeFinder_Custom, "Custom");
        m_pType->DefineEnumValue(LaserRangeFinder_Sick_LMS100, "Sick_LMS100");
        m_pType->DefineEnumValue(LaserRangeFinder_Sick_LMS200, "Sick_LMS200");
        m_pType->DefineEnumValue(LaserRangeFinder_Sick_LMS291, "Sick_LMS291");
        m_pType->DefineEnumValue(LaserRangeFinder_Hokuyo_UTM_30LX, "Hokuyo_UTM_30LX");
        m_pType->DefineEnumValue(LaserRangeFinder_Hokuyo_URG_04LX, "Hokuyo_URG_04LX");
    }

    /**
    * Set the number of range readings based on the minimum and
    * maximum angles of the sensor and the angular resolution
    */
    void Update()
    {
        m_NumberOfRangeReadings = static_cast<kt_int32u>(math::Round(
                (GetMaximumAngle() - GetMinimumAngle()) / GetAngularResolution()) + 1);
    }

    LaserRangeFinder(const LaserRangeFinder &);

    const LaserRangeFinder &operator=(const LaserRangeFinder &);

    // sensor m_Parameters
    Parameter<kt_double> *m_pMinimumAngle;
    Parameter<kt_double> *m_pMaximumAngle;
    Parameter<kt_double> *m_pAngularResolution;
    Parameter<kt_double> *m_pMinimumRange;
    Parameter<kt_double> *m_pMaximumRange;
    Parameter<kt_double> *m_pRangeThreshold;
    ParameterEnum *m_pType;
    kt_int32u m_NumberOfRangeReadings;

// static std::string LaserRangeFinderTypeNames[6];
};  // LaserRangeFinder

////////////////////////////////////////////////////////////////////////////////////////
/**
* Enumerated type for valid grid cell states
*/
typedef enum
{
    GridStates_Unknown = 0,
    GridStates_Occupied = 100,
    GridStates_Free = 255
} GridStates;

////////////////////////////////////////////////////////////////////////////////////////
/**
* The CoordinateConverter class is used to convert coordinates between world and grid coordinates
* In world coordinates 1.0 = 1 meter where 1 in grid coordinates = 1 pixel!
* Default scale for coordinate converter is 20 that converters to 1 pixel = 0.05 meter
*/
class CoordinateConverter
{
public:
    CoordinateConverter() : m_Scale(20.0) {}

    inline kt_double Transform(kt_double value) { return value * m_Scale; }

    /**
    * Converts the point from world coordinates to grid coordinates
    * @param rWorld world coordinate
    * @param flipY
    * @return grid coordinate
    */
    inline Vector2<kt_int32s> WorldToGrid(const Vector2<kt_double> &rWorld, kt_bool flipY = false) const
    {
        kt_double gridX = (rWorld.GetX() - m_Offset.GetX()) * m_Scale;
        kt_double gridY = 0.0;
        if (flipY == false) gridY = (rWorld.GetY() - m_Offset.GetY()) * m_Scale;
        else gridY = (m_Size.GetHeight() / m_Scale - rWorld.GetY() + m_Offset.GetY()) * m_Scale;
        return Vector2<kt_int32s>(static_cast<kt_int32s>(math::Round(gridX)),
                                  static_cast<kt_int32s>(math::Round(gridY)));
    }

    /**
    * Converts the point from grid coordinates to world coordinates
    * @param rGrid world coordinate
    * @param flipY
    * @return world coordinate
    */
    inline Vector2<kt_double> GridToWorld(const Vector2<kt_int32s> &rGrid, kt_bool flipY = false) const
    {
        kt_double worldX = m_Offset.GetX() + rGrid.GetX() / m_Scale;
        kt_double worldY = 0.0;
        if (flipY == false) worldY = m_Offset.GetY() + rGrid.GetY() / m_Scale;
        else worldY = m_Offset.GetY() + (m_Size.GetHeight() - rGrid.GetY()) / m_Scale;
        return Vector2<kt_double>(worldX, worldY);
    }

    inline kt_double GetScale() const { return m_Scale; }

    inline void SetScale(kt_double scale) { m_Scale = scale; }

    inline const Vector2<kt_double> &GetOffset() const { return m_Offset; }

    inline void SetOffset(const Vector2<kt_double> &rOffset) { m_Offset = rOffset; }

    inline void SetSize(const Size2<kt_int32s> &rSize) { m_Size = rSize; }

    inline const Size2<kt_int32s> &GetSize() const { return m_Size; }

    inline kt_double GetResolution() const { return 1.0 / m_Scale; }

    inline void SetResolution(kt_double resolution) { m_Scale = 1.0 / resolution; }

    inline BoundingBox2 GetBoundingBox() const
    {
        BoundingBox2 box;
        kt_double minX = GetOffset().GetX();
        kt_double minY = GetOffset().GetY();
        kt_double maxX = minX + GetSize().GetWidth() * GetResolution();
        kt_double maxY = minY + GetSize().GetHeight() * GetResolution();
        box.SetMinimum(GetOffset());
        box.SetMaximum(Vector2<kt_double>(maxX, maxY));
        return box;
    }

private:
    Size2<kt_int32s> m_Size;
    kt_double m_Scale;
    Vector2<kt_double> m_Offset;
};  // CoordinateConverter

////////////////////////////////////////////////////////////////////////////////////////
/**
* Defines a grid class
*/
template<typename T>
class Grid
{
public:
    /**
    * Creates a grid of given size and resolution
    * @param width
    * @param height
    * @param resolution
    * @return grid pointer
    */
    static Grid *CreateGrid(kt_int32s width, kt_int32s height, kt_double resolution)
    {
        Grid *pGrid = new Grid(width, height);
        pGrid->GetCoordinateConverter()->SetScale(1.0 / resolution);
        return pGrid;
    }

    virtual ~Grid()
    {
        delete[] m_pData;
        delete m_pCoordinateConverter;
    }

    void Clear() { memset(m_pData, 0, GetDataSize() * sizeof(T)); }

    Grid *Clone()
    {
        Grid *pGrid = CreateGrid(GetWidth(), GetHeight(), GetResolution());
        pGrid->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset());
        memcpy(pGrid->GetDataPointer(), GetDataPointer(), GetDataSize());
        return pGrid;
    }

    virtual void Resize(kt_int32s width, kt_int32s height)
    {
        m_Width = width;
        m_Height = height;
        m_WidthStep = math::AlignValue<kt_int32s>(width, 8);
        if (m_pData != NULL)
        {
            delete[] m_pData;
            m_pData = NULL;
        }
        try
        {
            m_pData = new T[GetDataSize()];
            if (m_pCoordinateConverter == NULL)
            {
                m_pCoordinateConverter = new CoordinateConverter();
            }
            m_pCoordinateConverter->SetSize(Size2<kt_int32s>(width, height));
        }
        catch (...)
        {
            m_pData = NULL;
            m_Width = 0;
            m_Height = 0;
            m_WidthStep = 0;
        }
        Clear();
    }

    /**
    * Checks whether the given coordinates are valid grid indices
    * @param rGrid
    */
    inline kt_bool IsValidGridIndex(const Vector2<kt_int32s> &rGrid) const
    {
        return (math::IsUpTo(rGrid.GetX(), m_Width) && math::IsUpTo(rGrid.GetY(), m_Height));
    }

    /**
    * Gets the index into the data pointer of the given grid coordinate
    * @param rGrid
    * @param boundaryCheck default value is true
    * @return grid index
    */
    virtual kt_int32s GridIndex(const Vector2<kt_int32s> &rGrid, kt_bool boundaryCheck = true) const
    {
        if (boundaryCheck)
        {
            if (!IsValidGridIndex(rGrid))
            {
                std::stringstream error;
                error << "Index " << rGrid << " out of range.  Index must be between [0; "
                      << m_Width << ") and [0; " << m_Height << ")";
                throw Exception(error.str());
            }
        }
        kt_int32s index = rGrid.GetX() + (rGrid.GetY() * m_WidthStep);
        if (boundaryCheck) assert(math::IsUpTo(index, GetDataSize()));
        return index;
    }

    /**
    * Gets the grid coordinate from an index
    * @param index
    * @return grid coordinate
    */
    Vector2<kt_int32s> IndexToGrid(kt_int32s index) const
    {
        Vector2<kt_int32s> grid;
        grid.SetY(index / m_WidthStep);
        grid.SetX(index - grid.GetY() * m_WidthStep);
        return grid;
    }

    /**
    * Converts the point from world coordinates to grid coordinates
    * @param rWorld world coordinate
    * @param flipY
    * @return grid coordinate
    */
    inline Vector2<kt_int32s> WorldToGrid(const Vector2<kt_double> &rWorld, kt_bool flipY = false) const
    {
        return GetCoordinateConverter()->WorldToGrid(rWorld, flipY);
    }

    /**
    * Converts the point from grid coordinates to world coordinates
    * @param rGrid world coordinate
    * @param flipY
    * @return world coordinate
    */
    inline Vector2<kt_double> GridToWorld(const Vector2<kt_int32s> &rGrid, kt_bool flipY = false) const
    {
        return GetCoordinateConverter()->GridToWorld(rGrid, flipY);
    }

    /**
    * Gets pointer to data at given grid coordinate
    * @param rGrid grid coordinate
    * @return grid point
    */
    T *GetDataPointer(const Vector2<kt_int32s> &rGrid)
    {
        kt_int32s index = GridIndex(rGrid, true);
        return m_pData + index;
    }

    /**
    * Gets pointer to data at given grid coordinate
    * @param rGrid grid coordinate
    * @return grid point
    */
    T *GetDataPointer(const Vector2<kt_int32s> &rGrid) const
    {
        kt_int32s index = GridIndex(rGrid, true);
        return m_pData + index;
    }

    inline kt_int32s GetWidth() const { return m_Width; };

    inline kt_int32s GetHeight() const { return m_Height; };

    inline const Size2<kt_int32s> GetSize() const { return Size2<kt_int32s>(m_Width, m_Height); }

    inline kt_int32s GetWidthStep() const { return m_WidthStep; }

    inline T *GetDataPointer() { return m_pData; }

    inline T *GetDataPointer() const { return m_pData; }

    inline kt_int32s GetDataSize() const { return m_WidthStep * m_Height; }

    inline T GetValue(const Vector2<kt_int32s> &rGrid) const { kt_int32s index = GridIndex(rGrid);return m_pData[index]; }

    inline CoordinateConverter *GetCoordinateConverter() const { return m_pCoordinateConverter; }

    inline kt_double GetResolution() const { return GetCoordinateConverter()->GetResolution(); }

    inline BoundingBox2 GetBoundingBox() const { return GetCoordinateConverter()->GetBoundingBox(); }

    void TraceLine(kt_int32s x0, kt_int32s y0, kt_int32s x1, kt_int32s y1, Functor *f = NULL)
    {
        kt_bool steep = abs(y1 - y0) > abs(x1 - x0);
        if (steep)
        {
            std::swap(x0, y0);
            std::swap(x1, y1);
        }
        if (x0 > x1)
        {
            std::swap(x0, x1);
            std::swap(y0, y1);
        }
        kt_int32s deltaX = x1 - x0;
        kt_int32s deltaY = abs(y1 - y0);
        kt_int32s error = 0;
        kt_int32s ystep;
        kt_int32s y = y0;
        if (y0 < y1) ystep = 1;
        else ystep = -1;
        kt_int32s pointX;
        kt_int32s pointY;
        for (kt_int32s x = x0; x <= x1; x++)
        {
            if (steep)
            {
                pointX = y;
                pointY = x;
            }
            else
            {
                pointX = x;
                pointY = y;
            }
            error += deltaY;
            if (2 * error >= deltaX)
            {
                y += ystep;
                error -= deltaX;
            }
            Vector2<kt_int32s> gridIndex(pointX, pointY);
            if (IsValidGridIndex(gridIndex))
            {
                kt_int32s index = GridIndex(gridIndex, false);
                T *pGridPointer = GetDataPointer();
                pGridPointer[index]++;
                if (f != NULL) (*f)(index);
            }
        }
    }

protected:
    Grid(kt_int32s width, kt_int32s height) : m_pData(NULL), m_pCoordinateConverter(NULL)
    {
        Resize(width, height);
    }

private:
    kt_int32s m_Width;       // width of grid
    kt_int32s m_Height;      // height of grid
    kt_int32s m_WidthStep;   // 8 bit aligned width of grid
    T *m_pData;              // grid data
    // coordinate converter to convert between world coordinates and grid coordinates
    CoordinateConverter *m_pCoordinateConverter;
};  // Grid

////////////////////////////////////////////////////////////////////////////////////////
/**
* For making custom data
*/
class CustomData : public Object
{
public:
// @cond EXCLUDE
    KARTO_Object(CustomData)
// @endcond
    CustomData() : Object() {}

    virtual ~CustomData() {}

    /**
    * Write out this custom data as a string
    * @return string representation of this data object
    */
    virtual const std::string Write() const = 0;

    /**
    * Read in this custom data from a string
    * @param rValue string representation of this data object
    */
    virtual void Read(const std::string &rValue) = 0;

private:
    CustomData(const CustomData &);

    const CustomData &operator=(const CustomData &);
};

/**
* Type declaration of CustomData vector
*/
typedef std::vector<CustomData *> CustomDataVector;

////////////////////////////////////////////////////////////////////////////////////////
/**
* SensorData is a base class for all sensor data
*/
class KARTO_EXPORT SensorData : public Object
{
public:
// @cond EXCLUDE
    KARTO_Object(SensorData)
// @endcond
    virtual ~SensorData();

    inline kt_int32s GetStateId() const { return m_StateId; }

    inline void SetStateId(kt_int32s stateId) { m_StateId = stateId; }

    inline kt_int32s GetUniqueId() const { return m_UniqueId; }

    inline void SetUniqueId(kt_int32u uniqueId) { m_UniqueId = uniqueId; }

    inline kt_double GetTime() const { return m_Time; }

    inline void SetTime(kt_double time) { m_Time = time; }

    inline const Name &GetSensorName() const { return m_SensorName; }

    inline void AddCustomData(CustomData *pCustomData)
    {
        m_CustomData.push_back(pCustomData);
    }

    inline const CustomDataVector &GetCustomData() const { return m_CustomData; }

protected:
/**
* Construct a SensorData object with a sensor name
*/
    SensorData(const Name &rSensorName);

private:
    SensorData(const SensorData &);

    const SensorData &operator=(const SensorData &);

    // ID unique to individual sensor
    kt_int32s m_StateId;
    // ID unique across all sensor data
    kt_int32s m_UniqueId;
    // Sensor that created this sensor data
    Name m_SensorName;
    // Time the sensor data was created
    kt_double m_Time;
    CustomDataVector m_CustomData;
};

////////////////////////////////////////////////////////////////////////////////////////
/**
* Type declaration of range readings vector
*/
typedef std::vector<kt_double> RangeReadingsVector;

/**
* LaserRangeScan representing the range readings from a laser range finder sensor.
*/
class LaserRangeScan : public SensorData
{
public:
// @cond EXCLUDE
    KARTO_Object(LaserRangeScan)
// @endcond

    LaserRangeScan(const Name &rSensorName)
            : SensorData(rSensorName), m_pRangeReadings(NULL), m_NumberOfRangeReadings(0) {}

    LaserRangeScan(const Name &rSensorName, const RangeReadingsVector &rRangeReadings)
            : SensorData(rSensorName), m_pRangeReadings(NULL), m_NumberOfRangeReadings(0)
    {
        assert(rSensorName.ToString() != "");
        SetRangeReadings(rRangeReadings);
    }

    virtual ~LaserRangeScan() { delete[] m_pRangeReadings; }

    inline kt_double *GetRangeReadings() const { return m_pRangeReadings; }

    inline RangeReadingsVector GetRangeReadingsVector() const
    {
        return RangeReadingsVector(m_pRangeReadings, m_pRangeReadings + m_NumberOfRangeReadings);
    }

    inline void SetRangeReadings(const RangeReadingsVector &rRangeReadings)
    {
        // ignore for now! XXXMAE BUGUBUG 05/21/2010 << TODO(lucbettaieb): What the heck is this??
        // if (rRangeReadings.size() != GetNumberOfRangeReadings())
        // {
        //   std::stringstream error;
        //   error << "Given number of readings (" << rRangeReadings.size()
        //         << ") does not match expected number of range finder (" << GetNumberOfRangeReadings() << ")";
        //   throw Exception(error.str());
        // }
        if (!rRangeReadings.empty())
        {
            if (rRangeReadings.size() != m_NumberOfRangeReadings)
            {
                // delete old readings
                delete[] m_pRangeReadings;
                // store size of array!
                m_NumberOfRangeReadings = static_cast<kt_int32u>(rRangeReadings.size());
                // allocate range readings
                m_pRangeReadings = new kt_double[m_NumberOfRangeReadings];
            }
            // copy readings
            kt_int32u index = 0;
            const_forEach(RangeReadingsVector, &rRangeReadings)
            {
                m_pRangeReadings[index++] = *iter;
            }
        }
        else
        {
            delete[] m_pRangeReadings;
            m_pRangeReadings = NULL;
        }
    }

    /**
    * Gets the laser range finder sensor that generated this scan
    * @return laser range finder sensor of this scan
    */
    inline LaserRangeFinder *GetLaserRangeFinder() const
    {
        return SensorManager::GetInstance()->GetSensorByName<LaserRangeFinder>(GetSensorName());
    }

    /**
    * Gets the number of range readings
    * @return number of range readings
    */
    inline kt_int32u GetNumberOfRangeReadings() const
    {
        return m_NumberOfRangeReadings;
    }

private:
    LaserRangeScan(const LaserRangeScan &);

    const LaserRangeScan &operator=(const LaserRangeScan &);

    kt_double *m_pRangeReadings;
    kt_int32u m_NumberOfRangeReadings;
};  // LaserRangeScan

////////////////////////////////////////////////////////////////////////////////////////
/**
* DrivePose representing the pose value of a drive sensor.
*/
class DrivePose : public SensorData
{
public:
// @cond EXCLUDE
    KARTO_Object(DrivePose)
// @endcond

    DrivePose(const Name &rSensorName) : SensorData(rSensorName) {}

    virtual ~DrivePose() {}

    inline const Pose3 &GetOdometricPose() const
    {
        return m_OdometricPose;
    }

    inline void SetOdometricPose(const Pose3 &rPose)
    {
        m_OdometricPose = rPose;
    }

private:
    DrivePose(const DrivePose &);

    const DrivePose &operator=(const DrivePose &);

    // Odometric pose of robot
    Pose3 m_OdometricPose;
};  // class DrivePose

////////////////////////////////////////////////////////////////////////////////////////
/**
* The LocalizedRangeScan contains range data from a single sweep of a laser range finder sensor
* in a two-dimensional space and position information. The odometer position is the position
* reported by the robot when the range data was recorded. The corrected position is the position
* calculated by the mapper (or localizer)
*/
class LocalizedRangeScan : public LaserRangeScan
{
public:
// @cond EXCLUDE
    KARTO_Object(LocalizedRangeScan)
// @endcond
    LocalizedRangeScan(const Name &rSensorName, const RangeReadingsVector &rReadings)
            : LaserRangeScan(rSensorName, rReadings), m_IsDirty(true) {}

    virtual ~LocalizedRangeScan() {}

    inline const Pose2 &GetOdometricPose() const
    {
        return m_OdometricPose;
    }

    inline void SetOdometricPose(const Pose2 &rPose)
    {
        m_OdometricPose = rPose;
    }

    inline const Pose2 &GetCorrectedPose() const
    {
        return m_CorrectedPose;
    }

    inline void SetCorrectedPose(const Pose2 &rPose)
    {
        m_CorrectedPose = rPose;
        m_IsDirty = true;
    }

    inline const Pose2 &GetBarycenterPose() const
    {
        boost::shared_lock<boost::shared_mutex> lock(m_Lock);
        if (m_IsDirty)
        {
            // throw away constness and do an update!
            lock.unlock();
            boost::unique_lock<boost::shared_mutex> uniqueLock(m_Lock);
            const_cast<LocalizedRangeScan *>(this)->Update();
        }
        return m_BarycenterPose;
    }

    /**
    * Gets barycenter if the given parameter is true, otherwise returns the scanner pose
    * @param useBarycenter
    * @return barycenter if parameter is true, otherwise scanner pose
    */
    inline Pose2 GetReferencePose(kt_bool useBarycenter) const
    {
        boost::shared_lock<boost::shared_mutex> lock(m_Lock);
        if (m_IsDirty)
        {
            // throw away constness and do an update!
            lock.unlock();
            boost::unique_lock<boost::shared_mutex> uniqueLock(m_Lock);
            const_cast<LocalizedRangeScan *>(this)->Update();
        }
        return useBarycenter ? GetBarycenterPose() : GetSensorPose();
    }

    /**
    * Computes the position of the sensor
    * @return scan pose
    */
    inline Pose2 GetSensorPose() const
    {
        return GetSensorAt(m_CorrectedPose);
    }

    /**
    * Computes the robot pose given the corrected scan pose
    * @param rScanPose pose of the sensor
    */
    void SetSensorPose(const Pose2 &rScanPose)
    {
        Pose2 deviceOffsetPose2 = GetLaserRangeFinder()->GetOffsetPose();
        kt_double offsetLength = deviceOffsetPose2.GetPosition().Length();
        kt_double offsetHeading = deviceOffsetPose2.GetHeading();
        kt_double angleoffset = atan2(deviceOffsetPose2.GetY(), deviceOffsetPose2.GetX());
        kt_double correctedHeading = math::NormalizeAngle(rScanPose.GetHeading());
        Pose2 worldSensorOffset = Pose2(offsetLength * cos(correctedHeading + angleoffset - offsetHeading),
                                        offsetLength * sin(correctedHeading + angleoffset - offsetHeading),
                                        offsetHeading);
        m_CorrectedPose = rScanPose - worldSensorOffset;
        Update();
    }

    /**
    * Computes the position of the sensor if the robot were at the given pose
    * @param rPose
    * @return sensor pose
    */
    inline Pose2 GetSensorAt(const Pose2 &rPose) const
    {
        return Transform(rPose).TransformPose(GetLaserRangeFinder()->GetOffsetPose());
    }

    /**
    * Gets the bounding box of this scan
    * @return bounding box of this scan
    */
    inline const BoundingBox2 &GetBoundingBox() const
    {
        boost::shared_lock<boost::shared_mutex> lock(m_Lock);
        if (m_IsDirty)
        {
            // throw away constness and do an update!
            lock.unlock();
            boost::unique_lock<boost::shared_mutex> uniqueLock(m_Lock);
            const_cast<LocalizedRangeScan *>(this)->Update();
        }
        return m_BoundingBox;
    }

    /**
    * Get point readings in local coordinates
    */
    inline const PointVectorDouble &GetPointReadings(kt_bool wantFiltered = false) const
    {
        boost::shared_lock<boost::shared_mutex> lock(m_Lock);
        if (m_IsDirty)
        {
            // throw away constness and do an update!
            lock.unlock();
            boost::unique_lock<boost::shared_mutex> uniqueLock(m_Lock);
            const_cast<LocalizedRangeScan *>(this)->Update();
        }
        if (wantFiltered) return m_PointReadings;
        else return m_UnfilteredPointReadings;
    }

private:
    /**
    * Compute point readings based on range readings
    * Only range readings within [minimum range; range threshold] are returned
    */
    virtual void Update()
    {
        LaserRangeFinder *pLaserRangeFinder = GetLaserRangeFinder();
        if (pLaserRangeFinder != NULL)
        {
            m_PointReadings.clear();
            m_UnfilteredPointReadings.clear();
            kt_double rangeThreshold = pLaserRangeFinder->GetRangeThreshold();
            kt_double minimumAngle = pLaserRangeFinder->GetMinimumAngle();
            kt_double angularResolution = pLaserRangeFinder->GetAngularResolution();
            Pose2 scanPose = GetSensorPose();
            // compute point readings
            Vector2<kt_double> rangePointsSum;
            kt_int32u beamNum = 0;
            for (kt_int32u i = 0; i < pLaserRangeFinder->GetNumberOfRangeReadings(); i++, beamNum++)
            {
                kt_double rangeReading = GetRangeReadings()[i];
                if (!math::InRange(rangeReading, pLaserRangeFinder->GetMinimumRange(), rangeThreshold))
                {
                    kt_double angle = scanPose.GetHeading() + minimumAngle + beamNum * angularResolution;
                    Vector2<kt_double> point;
                    point.SetX(scanPose.GetX() + (rangeReading * cos(angle)));
                    point.SetY(scanPose.GetY() + (rangeReading * sin(angle)));
                    m_UnfilteredPointReadings.push_back(point);
                    continue;
                }
                kt_double angle = scanPose.GetHeading() + minimumAngle + beamNum * angularResolution;
                Vector2<kt_double> point;
                point.SetX(scanPose.GetX() + (rangeReading * cos(angle)));
                point.SetY(scanPose.GetY() + (rangeReading * sin(angle)));
                m_PointReadings.push_back(point);
                m_UnfilteredPointReadings.push_back(point);
                rangePointsSum += point;
            }
            // compute barycenter
            kt_double nPoints = static_cast<kt_double>(m_PointReadings.size());
            if (nPoints != 0.0)
            {
                Vector2<kt_double> averagePosition = Vector2<kt_double>(rangePointsSum / nPoints);
                m_BarycenterPose = Pose2(averagePosition, 0.0);
            }
            else m_BarycenterPose = scanPose;
            // calculate bounding box of scan
            m_BoundingBox = BoundingBox2();
            m_BoundingBox.Add(scanPose.GetPosition());
            forEach(PointVectorDouble, &m_PointReadings)
            {
                m_BoundingBox.Add(*iter);
            }
        }
        m_IsDirty = false;
    }

    LocalizedRangeScan(const LocalizedRangeScan &);

    const LocalizedRangeScan &operator=(const LocalizedRangeScan &);

    mutable boost::shared_mutex m_Lock;
    // Odometric pose of robot
    Pose2 m_OdometricPose;
    // Corrected pose of robot calculated by mapper (or localizer)
    Pose2 m_CorrectedPose;
protected:
    // Average of all the point readings
    Pose2 m_BarycenterPose;
    // Vector of point readings
    PointVectorDouble m_PointReadings;
    // Vector of unfiltered point readings
    PointVectorDouble m_UnfilteredPointReadings;
    // Bounding box of localized range scan
    BoundingBox2 m_BoundingBox;
    // Internal flag used to update point readings, barycenter and bounding box
    kt_bool m_IsDirty;
};  // LocalizedRangeScan

/**
* Type declaration of LocalizedRangeScan vector
*/
typedef std::vector<LocalizedRangeScan *> LocalizedRangeScanVector;

////////////////////////////////////////////////////////////////////////////////////////
/**
* The LocalizedRangeScanWithPoints is an extension of the LocalizedRangeScan with precomputed points.
*/
class LocalizedRangeScanWithPoints : public LocalizedRangeScan
{
public:
// @cond EXCLUDE
    KARTO_Object(LocalizedRangeScanWithPoints)
// @endcond
    LocalizedRangeScanWithPoints(const Name &rSensorName, const RangeReadingsVector &rReadings,
                                 const PointVectorDouble &rPoints)
            : LocalizedRangeScan(rSensorName, rReadings), m_Points(rPoints) {}

    virtual ~LocalizedRangeScanWithPoints() {}

private:
    /**
    * Update the points based on the latest sensor pose.
    */
    void Update()
    {
        m_PointReadings.clear();
        m_UnfilteredPointReadings.clear();
        Pose2 scanPose = GetSensorPose();
        Pose2 robotPose = GetCorrectedPose();
        // update point readings
        Vector2<kt_double> rangePointsSum;
        for (kt_int32u i = 0; i < m_Points.size(); i++)
        {
            // check if this has a NaN
            if (!std::isfinite(m_Points[i].GetX()) || !std::isfinite(m_Points[i].GetY()))
            {
                Vector2<kt_double> point(m_Points[i].GetX(), m_Points[i].GetY());
                m_UnfilteredPointReadings.push_back(point);
                continue;
            }
            // transform into world coords
            Pose2 pointPose(m_Points[i].GetX(), m_Points[i].GetY(), 0);
            Pose2 result = Transform(robotPose).TransformPose(pointPose);
            Vector2<kt_double> point(result.GetX(), result.GetY());
            m_PointReadings.push_back(point);
            m_UnfilteredPointReadings.push_back(point);
            rangePointsSum += point;
        }
        // compute barycenter
        kt_double nPoints = static_cast<kt_double>(m_PointReadings.size());
        if (nPoints != 0.0)
        {
            Vector2<kt_double> averagePosition = Vector2<kt_double>(rangePointsSum / nPoints);
            m_BarycenterPose = Pose2(averagePosition, 0.0);
        }
        else m_BarycenterPose = scanPose;
        // calculate bounding box of scan
        m_BoundingBox = BoundingBox2();
        m_BoundingBox.Add(scanPose.GetPosition());
        forEach(PointVectorDouble, &m_PointReadings)
        {
            m_BoundingBox.Add(*iter);
        }
        m_IsDirty = false;
    }

    LocalizedRangeScanWithPoints(const LocalizedRangeScanWithPoints &);

    const LocalizedRangeScanWithPoints &operator=(const LocalizedRangeScanWithPoints &);

    const PointVectorDouble m_Points;
};  // LocalizedRangeScanWithPoints

////////////////////////////////////////////////////////////////////////////////////////
class OccupancyGrid;

class KARTO_EXPORT CellUpdater : public Functor
{
public:
    CellUpdater(OccupancyGrid *pGrid) : m_pOccupancyGrid(pGrid) {}

    virtual ~CellUpdater() {}

    virtual void operator()(kt_int32u index);

private:
    OccupancyGrid *m_pOccupancyGrid;
};  // CellUpdater

/**
* Occupancy grid definition. See GridStates for possible grid values.
*/
class OccupancyGrid : public Grid<kt_int8u>
{
    friend class CellUpdater;

    friend class IncrementalOccupancyGrid;

public:
/**
* Constructs an occupancy grid of given size
* @param width
* @param height
* @param rOffset
* @param resolution
*/
    OccupancyGrid(kt_int32s width, kt_int32s height, const Vector2<kt_double> &rOffset, kt_double resolution)
            : Grid<kt_int8u>(width, height), m_pCellPassCnt(Grid<kt_int32u>::CreateGrid(0, 0, resolution)),
              m_pCellHitsCnt(Grid<kt_int32u>::CreateGrid(0, 0, resolution)), m_pCellUpdater(NULL)
    {
        m_pCellUpdater = new CellUpdater(this);
        if (karto::math::DoubleEqual(resolution, 0.0))
        {
            throw Exception("Resolution cannot be 0");
        }
        m_pMinPassThrough = new Parameter<kt_int32u>("MinPassThrough", 2);
        m_pOccupancyThreshold = new Parameter<kt_double>("OccupancyThreshold", 0.1);
        GetCoordinateConverter()->SetScale(1.0 / resolution);
        GetCoordinateConverter()->SetOffset(rOffset);
    }

    virtual ~OccupancyGrid()
    {
        delete m_pCellUpdater;
        delete m_pCellPassCnt;
        delete m_pCellHitsCnt;
        delete m_pMinPassThrough;
        delete m_pOccupancyThreshold;
    }

    /**
    * Create an occupancy grid from the given scans using the given resolution
    * @param rScans      所有历史帧集合
    * @param resolution  分辨率
    */
    static OccupancyGrid *CreateFromScans(const LocalizedRangeScanVector &rScans, kt_double resolution)
    {
        if (rScans.empty()) return NULL;
        kt_int32s width, height;
        Vector2<kt_double> offset;
        ComputeDimensions(rScans, resolution, width, height, offset);
        OccupancyGrid *pOccupancyGrid = new OccupancyGrid(width, height, offset, resolution);
        pOccupancyGrid->CreateFromScans(rScans);
        return pOccupancyGrid;
    }

    /**
    * Make a clone
    * @return occupancy grid clone
    */
    OccupancyGrid *Clone() const
    {
        OccupancyGrid *pOccupancyGrid = new OccupancyGrid(GetWidth(),
                                                          GetHeight(),
                                                          GetCoordinateConverter()->GetOffset(),
                                                          1.0 / GetCoordinateConverter()->GetScale());
        memcpy(pOccupancyGrid->GetDataPointer(), GetDataPointer(), GetDataSize());
        pOccupancyGrid->GetCoordinateConverter()->SetSize(GetCoordinateConverter()->GetSize());
        pOccupancyGrid->m_pCellPassCnt = m_pCellPassCnt->Clone();
        pOccupancyGrid->m_pCellHitsCnt = m_pCellHitsCnt->Clone();
        return pOccupancyGrid;
    }

    /**
    * Check if grid point is free
    * @param rPose
    * @return whether the cell at the given point is free space
    */
    virtual kt_bool IsFree(const Vector2<kt_int32s> &rPose) const
    {
        kt_int8u *pOffsets = reinterpret_cast<kt_int8u *>(GetDataPointer(rPose));
        if (*pOffsets == GridStates_Free) return true;
        return false;
    }

    /**
    * Casts a ray from the given point (up to the given max range)
    * and returns the distance to the closest obstacle
    * @param rPose2
    * @param maxRange
    * @return distance to closest obstacle
    */
    virtual kt_double RayCast(const Pose2 &rPose2, kt_double maxRange) const
    {
        double scale = GetCoordinateConverter()->GetScale();
        kt_double x = rPose2.GetX();
        kt_double y = rPose2.GetY();
        kt_double theta = rPose2.GetHeading();
        kt_double sinTheta = sin(theta);
        kt_double cosTheta = cos(theta);
        kt_double xStop = x + maxRange * cosTheta;
        kt_double xSteps = 1 + fabs(xStop - x) * scale;
        kt_double yStop = y + maxRange * sinTheta;
        kt_double ySteps = 1 + fabs(yStop - y) * scale;
        kt_double steps = math::Maximum(xSteps, ySteps);
        kt_double delta = maxRange / steps;
        kt_double distance = delta;
        for (kt_int32u i = 1; i < steps; i++)
        {
            kt_double x1 = x + distance * cosTheta;
            kt_double y1 = y + distance * sinTheta;
            Vector2<kt_int32s> gridIndex = WorldToGrid(Vector2<kt_double>(x1, y1));
            if (IsValidGridIndex(gridIndex) && IsFree(gridIndex)) distance = (i + 1) * delta;
            else break;
        }
        return (distance < maxRange) ? distance : maxRange;
    }

    /**
    * Sets the minimum number of beams that must pass through a cell before it
    * will be considered to be occupied or unoccupied.
    * This prevents stray beams from messing up the map.
    */
    void SetMinPassThrough(kt_int32u count)
    {
        m_pMinPassThrough->SetValue(count);
    }

    /**
    * Sets the minimum ratio of beams hitting cell to beams passing through
    * cell for cell to be marked as occupied.
    */
    void SetOccupancyThreshold(kt_double thresh)
    {
        m_pOccupancyThreshold->SetValue(thresh);
    }

protected:
    /**
    * Get cell hit grid
    * @return Grid<kt_int32u>*
    */
    virtual Grid<kt_int32u> *GetCellHitsCounts()
    {
        return m_pCellHitsCnt;
    }

    /**
    * Get cell pass grid
    * @return Grid<kt_int32u>*
    */
    virtual Grid<kt_int32u> *GetCellPassCounts()
    {
        return m_pCellPassCnt;
    }

    /**
    * Calculate grid dimensions from localized range scans
    * @param rScans
    * @param resolution
    * @param rWidth
    * @param rHeight
    * @param rOffset
    */
    static void ComputeDimensions(const LocalizedRangeScanVector &rScans,
                                  kt_double resolution,
                                  kt_int32s &rWidth,
                                  kt_int32s &rHeight,
                                  Vector2<kt_double> &rOffset)
    {
        BoundingBox2 boundingBox;
        const_forEach(LocalizedRangeScanVector, &rScans)
        {
            boundingBox.Add((*iter)->GetBoundingBox());
        }
        kt_double scale = 1.0 / resolution;
        Size2<kt_double> size = boundingBox.GetSize();
        rWidth = static_cast<kt_int32s>(math::Round(size.GetWidth() * scale));
        rHeight = static_cast<kt_int32s>(math::Round(size.GetHeight() * scale));
        rOffset = boundingBox.GetMinimum();
    }

    /**
    * 根据所有历史帧创建栅格地图
    * 1、遍历历史帧，对于每一帧计算机器人与激光点连线之间的栅格，更新穿过、击中次数
    * 2、基于栅格击中、穿过次数更新栅格状态
    * @param rScans 所有历史帧
    */
    virtual void CreateFromScans(const LocalizedRangeScanVector &rScans)
    {
        m_pCellPassCnt->Resize(GetWidth(), GetHeight());
        m_pCellPassCnt->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset());
        m_pCellHitsCnt->Resize(GetWidth(), GetHeight());
        m_pCellHitsCnt->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset());
        // 遍历历史帧
        const_forEach(LocalizedRangeScanVector, &rScans)
        {
            LocalizedRangeScan *pScan = *iter;
            // 处理一帧激光点，计算机器人与激光点连线之间的栅格，更新穿过、击中次数
            // 1、在栅格地图中，计算两个点连线之间的栅格，对应栅格位置处的穿过计数加1
            // 2、终点击中计数加1
            AddScan(pScan);
        }
        // 计数法更新栅格地图
        // 基于栅格击中、穿过次数更新栅格状态
        Update();
    }
    /**
    * Adds the scan's information to this grid's counters (optionally
    * update the grid's cells' occupancy status)
    * 处理一帧激光点，计算机器人与激光点连线之间的栅格，更新穿过、击中次数
    * 1、在栅格地图中，计算两个点连线之间的栅格，对应栅格位置处的穿过计数加1
    * 2、终点击中计数加1
    * @param pScan     历史帧
    * @param doUpdate  是否更新栅格占用状态
    * @return
    */
    virtual kt_bool AddScan(LocalizedRangeScan *pScan, kt_bool doUpdate = false)
    {
        kt_double rangeThreshold = pScan->GetLaserRangeFinder()->GetRangeThreshold();
        kt_double maxRange = pScan->GetLaserRangeFinder()->GetMaximumRange();
        kt_double minRange = pScan->GetLaserRangeFinder()->GetMinimumRange();
        // 当前帧位置
        Vector2<kt_double> scanPosition = pScan->GetSensorPose().GetPosition();
        // 当前帧激光点数据
        const PointVectorDouble &rPointReadings = pScan->GetPointReadings(false);
        kt_bool isAllInMap = true;
        // draw lines from scan position to all point readings
        // 遍历激光点
        int pointIndex = 0;
        const_forEachAs(PointVectorDouble, &rPointReadings, pointsIter)
        {
            Vector2<kt_double> point = *pointsIter;
            kt_double rangeReading = pScan->GetRangeReadings()[pointIndex];
            kt_bool isEndPointValid = rangeReading < (rangeThreshold - KT_TOLERANCE);
            // 无效激光点
            if (rangeReading <= minRange || rangeReading >= maxRange || std::isnan(rangeReading))
            {
                // ignore these readings
                pointIndex++;
                continue;
            }
                // 距离超出最大阈值，修改点位置到最大阈值位置
            else if (rangeReading >= rangeThreshold)
            {
                // trace up to range reading
                kt_double ratio = rangeThreshold / rangeReading;
                kt_double dx = point.GetX() - scanPosition.GetX();
                kt_double dy = point.GetY() - scanPosition.GetY();
                point.SetX(scanPosition.GetX() + ratio * dx);
                point.SetY(scanPosition.GetY() + ratio * dy);
            }
            // 计算栅格地图中两点连线之间的栅格，更新穿过、击中次数
            // 1、在栅格地图中，计算两个点连线之间的栅格，对应栅格位置处的穿过计数加1
            // 2、终点击中计数加1
            kt_bool isInMap = RayTrace(scanPosition, point, isEndPointValid, doUpdate);
            if (!isInMap) isAllInMap = false;
            pointIndex++;
        }
        return isAllInMap;
    }

    /**
    * Traces a beam from the start position to the end position marking
    * the bookkeeping arrays accordingly.
    * 计算栅格地图中两点连线之间的栅格，更新穿过、击中次数
    * 1、在栅格地图中，计算两个点连线之间的栅格，对应栅格位置处的穿过计数加1
    * 2、终点击中计数加1
    * @param rWorldFrom      起点
    * @param rWorldTo        终点
    * @param isEndPointValid 最大阈值范围内的点
    * @param doUpdate        是否立即更新栅格状态
    * @return
    */
    virtual kt_bool RayTrace(const Vector2<kt_double> &rWorldFrom,
                             const Vector2<kt_double> &rWorldTo,
                             kt_bool isEndPointValid,
                             kt_bool doUpdate = false)
    {
        assert(m_pCellPassCnt != NULL && m_pCellHitsCnt != NULL);
        Vector2<kt_int32s> gridFrom = m_pCellPassCnt->WorldToGrid(rWorldFrom);
        Vector2<kt_int32s> gridTo = m_pCellPassCnt->WorldToGrid(rWorldTo);
        CellUpdater *pCellUpdater = doUpdate ? m_pCellUpdater : NULL;
        // 在栅格地图中，计算两个点连线之间的栅格，对应栅格位置处的pass计数加1
        m_pCellPassCnt->TraceLine(gridFrom.GetX(), gridFrom.GetY(), gridTo.GetX(), gridTo.GetY(), pCellUpdater);
        // 处理终点，要求点的距离小于最大阈值
        if (isEndPointValid)
        {
            if (m_pCellPassCnt->IsValidGridIndex(gridTo))
            {
                kt_int32s index = m_pCellPassCnt->GridIndex(gridTo, false);
                kt_int32u *pCellPassCntPtr = m_pCellPassCnt->GetDataPointer();
                kt_int32u *pCellHitCntPtr = m_pCellHitsCnt->GetDataPointer();
                // increment cell pass through and hit count
                // 穿过、击中次数+1
                pCellPassCntPtr[index]++;
                pCellHitCntPtr[index]++;
                if (doUpdate) (*m_pCellUpdater)(index);
            }
        }
        return m_pCellPassCnt->IsValidGridIndex(gridTo);
    }

    /**
    * 统计栅格击中、穿过次数，更新是否占用、未知
    * @param pCell       当前栅格
    * @param cellPassCnt 穿过的次数
    * @param cellHitCnt  击中的次数
    */
    virtual void UpdateCell(kt_int8u *pCell, kt_int32u cellPassCnt, kt_int32u cellHitCnt)
    {
        // 至少穿过2次，否则是未知点
        if (cellPassCnt > m_pMinPassThrough->GetValue())
        {
            // 击中/穿过
            kt_double hitRatio = static_cast<kt_double>(cellHitCnt) / static_cast<kt_double>(cellPassCnt);
            // 默认阈值是0.1
            if (hitRatio > m_pOccupancyThreshold->GetValue()) *pCell = GridStates_Occupied;
            else *pCell = GridStates_Free;
        }
    }

    /**
    * 计数法更新栅格地图
    * 基于栅格击中、穿过次数更新栅格状态
    */
    virtual void Update()
    {
        assert(m_pCellPassCnt != NULL && m_pCellHitsCnt != NULL);
        // clear grid
        Clear();
        // set occupancy status of cells
        kt_int8u *pDataPtr = GetDataPointer();
        kt_int32u *pCellPassCntPtr = m_pCellPassCnt->GetDataPointer();
        kt_int32u *pCellHitCntPtr = m_pCellHitsCnt->GetDataPointer();
        kt_int32u nBytes = GetDataSize();
        for (kt_int32u i = 0; i < nBytes; i++, pDataPtr++, pCellPassCntPtr++, pCellHitCntPtr++)
        {
            UpdateCell(pDataPtr, *pCellPassCntPtr, *pCellHitCntPtr);
        }
    }

    virtual void Resize(kt_int32s width, kt_int32s height)
    {
        Grid<kt_int8u>::Resize(width, height);
        m_pCellPassCnt->Resize(width, height);
        m_pCellHitsCnt->Resize(width, height);
    }

protected:
    // Counters of number of times a beam passed through a cell
    Grid<kt_int32u> *m_pCellPassCnt;
    // Counters of number of times a beam ended at a cell
    Grid<kt_int32u> *m_pCellHitsCnt;

private:
    OccupancyGrid(const OccupancyGrid &);

    const OccupancyGrid &operator=(const OccupancyGrid &);

    CellUpdater *m_pCellUpdater;

    ////////////////////////////////////////////////////////////
    // NOTE: These two values are dependent on the resolution.
    // If the resolution is too small, then not many beams will hit the cell!
    // Number of beams that must pass through a cell before it will be considered to be occupied or unoccupied.
    // This prevents stray beams from messing up the map.
    Parameter<kt_int32u> *m_pMinPassThrough;
    // Minimum ratio of beams hitting cell to beams passing through cell for cell to be marked as occupied
    Parameter<kt_double> *m_pOccupancyThreshold;
};  // OccupancyGrid

////////////////////////////////////////////////////////////////////////////////////////
/**
* Dataset info
* Contains title, author and other information about the dataset
*/
class DatasetInfo : public Object
{
public:
// @cond EXCLUDE
    KARTO_Object(DatasetInfo)
// @endcond
    DatasetInfo() : Object()
    {
        m_pTitle = new Parameter<std::string>("Title", "", GetParameterManager());
        m_pAuthor = new Parameter<std::string>("Author", "", GetParameterManager());
        m_pDescription = new Parameter<std::string>("Description", "", GetParameterManager());
        m_pCopyright = new Parameter<std::string>("Copyright", "", GetParameterManager());
    }

    virtual ~DatasetInfo() {}

    const std::string &GetTitle() const
    {
        return m_pTitle->GetValue();
    }

    const std::string &GetAuthor() const
    {
        return m_pAuthor->GetValue();
    }

    const std::string &GetDescription() const
    {
        return m_pDescription->GetValue();
    }

    const std::string &GetCopyright() const
    {
        return m_pCopyright->GetValue();
    }

private:
    DatasetInfo(const DatasetInfo &);
    const DatasetInfo &operator=(const DatasetInfo &);
    Parameter<std::string> *m_pTitle;
    Parameter<std::string> *m_pAuthor;
    Parameter<std::string> *m_pDescription;
    Parameter<std::string> *m_pCopyright;
};  // class DatasetInfo

////////////////////////////////////////////////////////////////////////////////////////
/**
* Karto dataset. Stores LaserRangeFinders and LocalizedRangeScans and manages memory of allocated LaserRangeFinders
* and LocalizedRangeScans
*/
class Dataset
{
public:
    Dataset() : m_pDatasetInfo(NULL) {}

    virtual ~Dataset() { Clear(); }

    void Add(Object *pObject)
    {
        if (pObject != NULL)
        {
            if (dynamic_cast<Sensor *>(pObject))
            {
                Sensor *pSensor = dynamic_cast<Sensor *>(pObject);
                if (pSensor != NULL)
                {
                    m_SensorNameLookup[pSensor->GetName()] = pSensor;
                    karto::SensorManager::GetInstance()->RegisterSensor(pSensor);
                }
                m_Objects.push_back(pObject);
            }
            else if (dynamic_cast<SensorData *>(pObject))
            {
                SensorData *pSensorData = dynamic_cast<SensorData *>(pObject);
                m_Objects.push_back(pSensorData);
            }
            else if (dynamic_cast<DatasetInfo *>(pObject))
            {
                m_pDatasetInfo = dynamic_cast<DatasetInfo *>(pObject);
            }
            else m_Objects.push_back(pObject);
        }
    }

    inline const ObjectVector &GetObjects() const { return m_Objects; }

    inline DatasetInfo *GetDatasetInfo() { return m_pDatasetInfo; }

    virtual void Clear()
    {
        for (std::map<Name, Sensor *>::iterator iter = m_SensorNameLookup.begin();
             iter != m_SensorNameLookup.end(); ++iter)
        {
            karto::SensorManager::GetInstance()->UnregisterSensor(iter->second);
        }
        forEach(ObjectVector, &m_Objects)
        {
            delete *iter;
        }
        m_Objects.clear();
        if (m_pDatasetInfo != NULL)
        {
            delete m_pDatasetInfo;
            m_pDatasetInfo = NULL;
        }
    }

private:
    std::map<Name, Sensor *> m_SensorNameLookup;
    ObjectVector m_Objects;
    DatasetInfo *m_pDatasetInfo;
};  // Dataset

////////////////////////////////////////////////////////////////////////////////////////
/**
* An array that can be resized as long as the size
* does not exceed the initial capacity
*/
class LookupArray
{
public:
    LookupArray() : m_pArray(NULL), m_Capacity(0), m_Size(0) {}

    virtual ~LookupArray()
    {
        assert(m_pArray != NULL);
        delete[] m_pArray;
        m_pArray = NULL;
    }

    void Clear() { memset(m_pArray, 0, sizeof(kt_int32s) * m_Capacity); }

    kt_int32u GetSize() const { return m_Size; }

    void SetSize(kt_int32u size)
    {
        assert(size != 0);
        if (size > m_Capacity)
        {
            delete[] m_pArray;
            m_Capacity = size;
            m_pArray = new kt_int32s[m_Capacity];
        }
        m_Size = size;
    }

    inline kt_int32s &operator[](kt_int32u index)
    {
        assert(index < m_Size);
        return m_pArray[index];
    }

    inline kt_int32s operator[](kt_int32u index) const
    {
        assert(index < m_Size);
        return m_pArray[index];
    }

    /**
    * Gets array pointer
    * @return array pointer
    */
    inline kt_int32s *GetArrayPointer() { return m_pArray; }

    /**
    * Gets array pointer
    * @return array pointer
    */
    inline kt_int32s *GetArrayPointer() const { return m_pArray; }

private:
    kt_int32s *m_pArray;
    kt_int32u m_Capacity;
    kt_int32u m_Size;
};  // LookupArray

////////////////////////////////////////////////////////////////////////////////////////
/**
* Create lookup tables for point readings at varying angles in grid.
* For each angle, grid indexes are calculated for each range reading.
* This is to speed up finding best angle/position for a localized range scan
*
* Used heavily in mapper and localizer.
*
* In the localizer, this is a huge speed up for calculating possible position.  For each particle,
* a probability is calculated.  The range scan is the same, but all grid indexes at all possible angles are
* calculated.  So when calculating the particle probability at a specific angle, the index table is used
* to look up probability in probability grid!
*
*/
template<typename T>
class GridIndexLookup
{
public:
    GridIndexLookup(Grid<T> *pGrid) : m_pGrid(pGrid), m_Capacity(0), m_Size(0), m_ppLookupArray(NULL) {}

    virtual ~GridIndexLookup() { DestroyArrays(); }

    /**
    * Gets the lookup array for a particular angle index
    * @param index
    * @return lookup array
    */
    const LookupArray *GetLookupArray(kt_int32u index) const
    {
        assert(math::IsUpTo(index, m_Size));
        return m_ppLookupArray[index];
    }

    /**
    * Get angles
    * @return std::vector<kt_double>& angles
    */
    const std::vector<kt_double> &GetAngles() const { return m_Angles; }

    /**
    * 创建索引表，计算当前帧激光点，在各个旋转角度变换下，新的激光点位置相对于当前帧位置（机器人的位置）的坐标偏移量
    * 不管当前帧在什么位置，只要旋转角度一定，那么坐标偏移量也是确定的。
    * 目的是后面在搜索空间中对当前帧位姿施加不同的x平移、y平移之后再施加一个旋转的操作，不用重新计算每个搜索位置处旋转某个角度之后对应的点坐标，只需要加上对应旋转角度下算好的坐标偏移量即可。
    * @param pScan           当前帧，搜索中心
    * @param angleCenter     当前帧朝向（0°朝向）
    * @param angleOffset     搜索角度范围，正负范围，这里是偏移量，范围的一半
    * @param angleResolution 搜索角度步长
    */
    void ComputeOffsets(LocalizedRangeScan *pScan,
                        kt_double angleCenter,
                        kt_double angleOffset,
                        kt_double angleResolution)
    {
        assert(angleOffset != 0.0);
        assert(angleResolution != 0.0);
        // 搜索角度数量
        kt_int32u nAngles = static_cast<kt_int32u>(math::Round(angleOffset * 2.0 / angleResolution) + 1);
        SetSize(nAngles);
        //////////////////////////////////////////////////////
        // convert points into local coordinates of scan pose
        // 当前帧激光点，在odom系下
        const PointVectorDouble &rPointReadings = pScan->GetPointReadings();
        // compute transform to scan pose lidar与odom的变换
        Transform transform(pScan->GetSensorPose());
        Pose2Vector localPoints;
        const_forEach(PointVectorDouble, &rPointReadings)
        {
            // do inverse transform to get points in local coordinates
            // 从odom系转换到局部lidar系下
            Pose2 vec = transform.InverseTransformPose(Pose2(*iter, 0.0));
            localPoints.push_back(vec);
        }
        //////////////////////////////////////////////////////
        // create lookup array for different angles
        // 对所有旋转角度，计算新的激光点位置相对于当前帧位置（机器人的位置）的坐标偏移量
        kt_double angle = 0.0;
        kt_double startAngle = angleCenter - angleOffset;
        for (kt_int32u angleIndex = 0; angleIndex < nAngles; angleIndex++)
        {
            angle = startAngle + angleIndex * angleResolution;
            // 计算当前帧激光点，在某旋转角度变换下，新的激光点位置相对于当前帧位置（机器人的位置）的坐标偏移量
            // 不管当前帧在什么位置，只要旋转角度一定，那么坐标偏移量也是确定的。
            ComputeOffsets(angleIndex, angle, localPoints, pScan);
        }
        // assert(math::DoubleEqual(angle, angleCenter + angleOffset));
    }

private:
    /**
    * 计算当前帧激光点，在某旋转角度变换下，新的激光点位置相对于当前帧位置（机器人的位置）的坐标偏移量
    * 不管当前帧在什么位置，只要旋转角度一定，那么坐标偏移量也是确定的。
    * @param angleIndex    角度数组索引
    * @param angle         旋转角度
    * @param rLocalPoints  当前帧在局部lidar系下的激光点
    */
    void ComputeOffsets(kt_int32u angleIndex, kt_double angle, const Pose2Vector &rLocalPoints,
                        LocalizedRangeScan *pScan)
    {
        m_ppLookupArray[angleIndex]->SetSize(static_cast<kt_int32u>(rLocalPoints.size()));
        m_Angles.at(angleIndex) = angle;
        // set up point array by computing relative offsets to points readings
        // when rotated by given angle
        const Vector2<kt_double> &rGridOffset = m_pGrid->GetCoordinateConverter()->GetOffset();
        kt_double cosine = cos(angle);
        kt_double sine = sin(angle);
        kt_int32u readingIndex = 0;
        // 当前旋转角度下的激光点索引 - 新点相对于原始点的网格索引偏移
        kt_int32s *pAngleIndexPointer = m_ppLookupArray[angleIndex]->GetArrayPointer();
        kt_double maxRange = pScan->GetLaserRangeFinder()->GetMaximumRange();
        // 遍历当前帧激光点
        const_forEach(Pose2Vector, &rLocalPoints)
        {
            // 激光点位置
            const Vector2<kt_double> &rPosition = iter->GetPosition();
            // 无效点
            if (std::isnan(pScan->GetRangeReadings()[readingIndex]) ||
                std::isinf(pScan->GetRangeReadings()[readingIndex]))
            {
                pAngleIndexPointer[readingIndex] = INVALID_SCAN;
                readingIndex++;
                continue;
            }
            // counterclockwise rotation and that rotation is about the origin (0, 0).
            // 施加旋转得到新点的坐标
            Vector2<kt_double> offset;
            offset.SetX(cosine * rPosition.GetX() - sine * rPosition.GetY());
            offset.SetY(sine * rPosition.GetX() + cosine * rPosition.GetY());
            // have to compensate for the grid offset when getting the grid index
            // 这里的offset是当前lidar帧坐标系下的坐标，不是world系坐标，那么直接调用WorldToGrid，实际上计算的是该点与当前帧位置之间的坐标偏移量
            // 注意是偏移量，目的是后面在搜索空间中对当前帧位姿施加不同的x平移、y平移之后施加一个旋转的操作，不用重新计算每个搜索位置处旋转某个角度之后对应的点坐标
            // 只需要加上对应旋转角度下算好的坐标偏移量即可。
            // 注：在任意位置下，旋转一定角度的激光点坐标偏移量都是相等的。
            Vector2<kt_int32s> gridPoint = m_pGrid->WorldToGrid(offset + rGridOffset);
            // use base GridIndex to ignore ROI
            // 网格索引偏移
            kt_int32s lookupIndex = m_pGrid->Grid<T>::GridIndex(gridPoint, false);
            // 存当前旋转角度下，激光点索引 - 新点相对于原始点的网格索引偏移
            pAngleIndexPointer[readingIndex] = lookupIndex;
            readingIndex++;
        }
        assert(readingIndex == rLocalPoints.size());
    }

    /**
    * Sets size of lookup table (resize if not big enough)
    * @param size
    */
    void SetSize(kt_int32u size)
    {
        assert(size != 0);
        if (size > m_Capacity)
        {
            if (m_ppLookupArray != NULL) DestroyArrays();
            m_Capacity = size;
            m_ppLookupArray = new LookupArray *[m_Capacity];
            for (kt_int32u i = 0; i < m_Capacity; i++)
            {
                m_ppLookupArray[i] = new LookupArray();
            }
        }
        m_Size = size;
        m_Angles.resize(size);
    }

    /**
    * Delete the arrays
    */
    void DestroyArrays()
    {
        for (kt_int32u i = 0; i < m_Capacity; i++) delete m_ppLookupArray[i];
        delete[] m_ppLookupArray;
        m_ppLookupArray = NULL;
    }

    Grid<T> *m_pGrid;
    kt_int32u m_Capacity;
    kt_int32u m_Size;
    LookupArray **m_ppLookupArray;
    // for sanity check
    std::vector<kt_double> m_Angles;
};  // class GridIndexLookup

////////////////////////////////////////////////////////////////////////////////////////
inline Pose2::Pose2(const Pose3 &rPose)
        : m_Position(rPose.GetPosition().GetX(), rPose.GetPosition().GetY())
{
    kt_double t1, t2;
    // calculates heading from orientation
    rPose.GetOrientation().ToEulerAngles(m_Heading, t1, t2);
}

////////////////////////////////////////////////////////////////////////////////////////
template<typename T>
inline void Object::SetParameter(const std::string &rName, T value)
{
    AbstractParameter *pParameter = GetParameter(rName);
    if (pParameter != NULL)
    {
        std::stringstream stream;
        stream << value;
        pParameter->SetValueFromString(stream.str());
    }
    else throw Exception("Parameter does not exist:  " + rName);
}

template<>
inline void Object::SetParameter(const std::string &rName, kt_bool value)
{
    AbstractParameter *pParameter = GetParameter(rName);
    if (pParameter != NULL) pParameter->SetValueFromString(value ? "true" : "false");
    else throw Exception("Parameter does not exist:  " + rName);
}

}  // namespace karto

#endif  // OPEN_KARTO_KARTO_H
